412 lines
13 KiB
C
412 lines
13 KiB
C
#pragma config(Sensor, in1, armPos, sensorPotentiometer)
|
|
#pragma config(Sensor, in2, lightR, sensorReflection)
|
|
#pragma config(Sensor, in3, lightL, sensorReflection)
|
|
#pragma config(Sensor, dgtl3, buttonStart, sensorDigitalIn)
|
|
#pragma config(Sensor, dgtl4, armLimit, sensorDigitalIn)
|
|
#pragma config(Sensor, dgtl5, buttonL, sensorDigitalIn)
|
|
#pragma config(Sensor, dgtl6, rotaryR1, sensorQuadEncoder)
|
|
#pragma config(Sensor, dgtl8, sonarIn, sensorSONAR_inch)
|
|
#pragma config(Sensor, dgtl10, rotaryL1, sensorQuadEncoder)
|
|
#pragma config(Sensor, dgtl12, buttonR, sensorDigitalIn)
|
|
#pragma config(Motor, port1, leftMotor, tmotorVex393_HBridge, openLoop, reversed, driveLeft)
|
|
#pragma config(Motor, port2, clawMotor, tmotorVex393_MC29, openLoop)
|
|
#pragma config(Motor, port3, armMotor, tmotorVex393_MC29, openLoop)
|
|
#pragma config(Motor, port10, rightMotor, tmotorVex393_HBridge, openLoop, reversed, driveRight)
|
|
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
|
|
|
|
//==================================================================================//
|
|
|
|
/* Progressive Development Code
|
|
* for ETC 244 VeX Robotics Lab
|
|
* Instructor: Bill Dolan
|
|
* Fall 2023-2024
|
|
*
|
|
* Developed by Skylar Grant
|
|
*
|
|
* Notes: This bot and its configuration are non-standard. My sensors are
|
|
* located in different locations than usual, and are plugged into
|
|
* different ports than usual.
|
|
*/
|
|
|
|
//==================================================================================//
|
|
|
|
/* Quick function to set the speed of the drive motors
|
|
*
|
|
* Arguments:
|
|
* - leftSpeed: signed integer, -127 to 127
|
|
* - Speed to drive the left-side motor
|
|
* - rightSpeed: signed integer, -127 to 127
|
|
* - Speed to drive the right-side motor
|
|
*/
|
|
void setMotors(int leftSpeed, int rightSpeed) {
|
|
// Turn on the left motor at the given speed
|
|
motor[leftMotor] = leftSpeed;
|
|
// Turn on the right motor at the given speed
|
|
motor[rightMotor] = rightSpeed;
|
|
}
|
|
|
|
/* Quickly set the arm motor speed
|
|
*
|
|
* Arguments:
|
|
* - speed: signed integer, -127 to 127
|
|
* - Speed to drive the arm motor
|
|
*
|
|
* Notes:
|
|
* This is a very superfluous function but I hate typing motor[motorName] = 127 constantly
|
|
*/
|
|
void setArm(int speed) {
|
|
// Set the arm motor speed
|
|
motor[armMotor] = speed;
|
|
}
|
|
|
|
/* Quickly set the claw motor speed
|
|
*
|
|
* Arguments:
|
|
* - speed: signed integer, -127 to 127
|
|
* - Speed to drive the claw motor
|
|
*
|
|
* Notes:
|
|
* This is a very superfluous function but I hate typing motor[motorName] = 127 constantly
|
|
*/
|
|
void setClaw(int speed) {
|
|
// Set the claw motor speed
|
|
motor[clawMotor] = speed;
|
|
}
|
|
|
|
//==================================================================================//
|
|
|
|
/* Quick function to stop both drive motors
|
|
*
|
|
* Arguments: None
|
|
*/
|
|
void stop() {
|
|
// Turn off the left motor
|
|
motor[leftMotor] = 0;
|
|
// Turn off the right motor
|
|
motor[rightMotor] = 0;
|
|
}
|
|
|
|
//==================================================================================//
|
|
|
|
/* Function to drive the bot forward for some time
|
|
*
|
|
* Arguments:
|
|
* - speed: signed integer, 0 to 127.
|
|
* - Speed to drive the motors
|
|
* - timeMs: signed integer
|
|
* - Amount of time to wait before stopping, in milliseconds
|
|
*/
|
|
void driveForward(int speed, int timeMs) {
|
|
// Turn on the left and right motors at speed
|
|
setMotors(speed, speed); // setMotors(leftSpeed, rightSpeed)
|
|
// Wait for the set time
|
|
wait1Msec(timeMs);
|
|
// Turn the motors off
|
|
stop();
|
|
}
|
|
|
|
//==================================================================================//
|
|
|
|
/* Function to drive the bot backwards for some time
|
|
*
|
|
* Arguments:
|
|
* - speed: signed integer, 0 to 127.
|
|
* - Speed to drive the motors, inversion is handled internally
|
|
* - timeMs: signed integer
|
|
* - Amount of time to wait before stopping, in milliseconds
|
|
*/
|
|
void driveBackward(int speed, int timeMs) {
|
|
// Invert the speed
|
|
int revSpeed = speed * -1;
|
|
// Turn on the left and right motors at the reverse speed
|
|
setMotors(revSpeed, revSpeed); // setMotors(leftSpeed, rightSpeed)
|
|
// Wait for the set time
|
|
wait1Msec(timeMs);
|
|
// Turn the motors off
|
|
stop();
|
|
}
|
|
|
|
//==================================================================================//
|
|
|
|
// TODO: Just combine this into one drive function with L/R/B and a reverse boolean
|
|
/* Function to drive only one side's drive motors for a given time
|
|
*
|
|
* Arguments:
|
|
* - side: 1 character string, "L" | "R"
|
|
* - Which side's motors to drive
|
|
* - speed: signed integer, 0 to 127.
|
|
* - Speed to drive the motors
|
|
* - timeMs: signed integer
|
|
* - Amount of time to wait before stopping, in milliseconds
|
|
*/
|
|
void driveOneSide(char *side, int speed, int timeMs) {
|
|
// Check the direction and apply the motor speeds as required
|
|
if (strcmp(side, "L") == 0) { // Drive Left Side
|
|
setMotors(speed, 0);
|
|
} else if (strcmp(side, "R") == 0) { // Drive Right Side
|
|
setMotors(0, speed); // setMotors(leftSpeed, rightSpeed)
|
|
}
|
|
// Wait for the specified amount of time
|
|
wait1Msec(timeMs);
|
|
// Turn off the motors
|
|
stop();
|
|
}
|
|
|
|
//==================================================================================//
|
|
|
|
// TODO: Flesh out this function
|
|
//void drive(char *side, int speed, int timeMs, int reverse)
|
|
|
|
/* Turn the robot
|
|
*
|
|
* Arguments:
|
|
* - direction: One character string, "L" | "R"
|
|
* - Which direction to turn
|
|
* - speed: signed integer 0 to 127
|
|
* - Speed to drive the motors at. Inversion is handled internally if needed
|
|
* - timeMs: signed integer
|
|
* - Milliseconds to wait before stopping the turn
|
|
* - reverse: boolean
|
|
* - Whether or not to reverse the turning-side motors to pivot-in-place
|
|
*/
|
|
void turn(char *direction, int speed, int timeMs, bool reverse) {
|
|
// Turn left by reversing the left drive motors and going forward
|
|
// with the right drive motors
|
|
// Turn right by going forward with the left drive motors
|
|
// and reversing the right drive motors
|
|
|
|
// Default to not reversing the turning-side motors
|
|
int revSpeed = 0;
|
|
|
|
// If reverse is true, reverse the turning-side motors
|
|
if (reverse == true) {
|
|
// Get the inverse of the speed for pivot-in-place
|
|
revSpeed = speed * -1;
|
|
}
|
|
|
|
// Check the direction and apply the motor speeds as required
|
|
if (strcmp(direction, "L") == 0) { // Turn Left
|
|
setMotors(revSpeed, speed); // setMotors(leftSpeed, rightSpeed)
|
|
} else if (strcmp(direction, "R") == 0) { // Turn Right
|
|
setMotors(speed, revSpeed); // setMotors(leftSpeed, rightSpeed)
|
|
} else {
|
|
wait1Msec(100);
|
|
}
|
|
// Wait for the specified amount of time
|
|
wait1Msec(timeMs);
|
|
// Turn off the motors
|
|
stop();
|
|
}
|
|
|
|
//==================================================================================//
|
|
|
|
/* Move the bot's arm up or down
|
|
*
|
|
* Arguments:
|
|
* - speed: signed integer. -127 to 127
|
|
* - <0: Down
|
|
* - >0: Up
|
|
* - Speed to drive the arm motor
|
|
* - timeMs: signed integer
|
|
* - Time to wait before stopping the arm, in milliseconds
|
|
*/
|
|
void moveArm(int speed, int timeMs) {
|
|
// Turn on the motor at the set speed
|
|
motor[armMotor] = speed;
|
|
// Wait for the set time
|
|
wait1Msec(timeMs);
|
|
// Turn the motor off
|
|
motor[armMotor] = 0;
|
|
}
|
|
|
|
/* Put your arms in the air like you just don't care
|
|
* Moves the bot's arm up until it reaches a preset position,
|
|
* determined by a potentiometer
|
|
*
|
|
* Arguments:
|
|
* - speed: signed integer. 0 to 127
|
|
* - Speed to drive the arm motor
|
|
*
|
|
* Logic:
|
|
* - Arm Position Potentiometer:
|
|
* - Claw Vertical: 1600
|
|
* - Claw Soft Limit: 1800
|
|
* - Claw Hard Limit: 2100
|
|
*/
|
|
void armUp100(int speed) {
|
|
// Check that the arm potentiometer is showing a value below 1800
|
|
while (SensorValue[armPos] <= 1800) {
|
|
// Set the motor to the desired speed
|
|
motor[armMotor] = speed;
|
|
}
|
|
// Once the arm position is showing > 1800, shut off the arm
|
|
motor[armMotor] = 0;
|
|
}
|
|
|
|
//==================================================================================//
|
|
|
|
/* Moves the robot's arm down until the limit switch is pressed
|
|
*
|
|
* Arguments:
|
|
* - speed: signed integer. 0 to 127
|
|
* - Speed to drive the arm motor
|
|
*
|
|
* Logic:
|
|
* - Arm Limit Switch:
|
|
* - Momentary switch, normally closed.
|
|
* - When arm is not making contact, shows a logical 1
|
|
*/
|
|
void armDown100(int speed) {
|
|
// While the arm limit switch isn't being contacted
|
|
while (SensorValue[armLimit] == 1) {
|
|
// Invert the given speed to make the motor go down
|
|
int revSpeed = speed * -1;
|
|
// Turn on the motor at the set speed
|
|
motor[armMotor] = revSpeed;
|
|
}
|
|
// Once the limit switch is being contacted (logical 0), stop the arm motor
|
|
motor[armMotor] = 0;
|
|
}
|
|
|
|
//==================================================================================//
|
|
|
|
/* Close the claw
|
|
*
|
|
* Arguments: None
|
|
* Notes: TODO: This should have some variability, and ideally
|
|
* would have a way to detect resistance against the claw
|
|
*/
|
|
void clawGrab() {
|
|
// Turn on the motor at the half speed
|
|
motor[clawMotor] = -32;
|
|
// Wait for the set time
|
|
wait1Msec(1000);
|
|
// Turn the motor off
|
|
motor[clawMotor] = 0;
|
|
}
|
|
|
|
//==================================================================================//
|
|
|
|
/* Open the claw
|
|
*
|
|
* Arguments: None
|
|
* Notes: TODO: This should have some variability, and ideally
|
|
* would have a way to detect the claw being open 100%
|
|
*/
|
|
void clawRelease() {
|
|
// Turn on the motor at the half speed
|
|
motor[clawMotor] = 32;
|
|
// Wait for the set time
|
|
wait1Msec(1000);
|
|
// Turn the motor off
|
|
motor[clawMotor] = 0;
|
|
}
|
|
|
|
//==================================================================================//
|
|
|
|
/* Run the autonomous code, called when the start button is pressed.
|
|
*
|
|
* Arguments: None
|
|
* Notes: TODO: Find a way to recall main() so this can be rerun without rebooting/recompiling
|
|
*/
|
|
void runAutonomous() {
|
|
// Drive Left Side
|
|
// driveOneSide("L", 127, 1000);
|
|
// Drive Right Side
|
|
// driveOneSide("R", 127, 1000);
|
|
// Reverse Left Side
|
|
// driveOneSide("L", -127, 1000);
|
|
// Reverse Right Side
|
|
// driveOneSide("R", -127, 1000);
|
|
// Drive forward 3 seconds
|
|
// driveForward(127, 3000);
|
|
// Drive backwards 3 seconds
|
|
// driveBackward(127, 3000);
|
|
// Left forward, right reverse
|
|
// turn("R", 127, 1000, true);
|
|
// Half-speed forward
|
|
// driveForward(64, 3000);
|
|
// Quarter-speed forward
|
|
// driveForward(32, 3000);
|
|
// Raise the arm up a bit
|
|
// armUp(127, 500);
|
|
// Drop the arm all the way down
|
|
// armDown100(16);
|
|
clawRelease();
|
|
clawGrab();
|
|
armUp100(32);
|
|
clawRelease();
|
|
clawGrab();
|
|
armDown100(32);
|
|
}
|
|
|
|
//==================================================================================//
|
|
|
|
/* Wait for the start button to be pressed, then run autonomous code.
|
|
*
|
|
* Arguments: None
|
|
*/
|
|
void waitForStart() {
|
|
|
|
// TODO: Experimental, hoping this will call this function again when done instead of exiting
|
|
waitForStart();
|
|
}
|
|
|
|
//==================================================================================//
|
|
|
|
// This function gets called automatically once the bot is booted.
|
|
task main() {
|
|
while (SensorValue[buttonStart]==1) {
|
|
// Driver Mode, Controller Inputs
|
|
int leftSpeed = (vexRT[Ch2] + vexRT[Ch1]) / 2; // (y + x) / 2
|
|
int rightSpeed = (vexRT[Ch2] - vexRT[Ch1])/2; // (y - x) / 2
|
|
float leftAdjSpeed = leftSpeed / 10;
|
|
float rightAdjSpeed = rightSpeed / 10;
|
|
leftAdjSpeed = floor(leftAdjSpeed) * 10;
|
|
rightAdjSpeed = floor(rightAdjSpeed) * 10;
|
|
|
|
if (fabs(leftAdjSpeed) > 30) {
|
|
motor[leftMotor] = leftAdjSpeed;
|
|
} else {
|
|
motor[leftMotor] = 0;
|
|
}
|
|
|
|
if (fabs(rightAdjSpeed) > 30) {
|
|
motor[rightMotor] = rightAdjSpeed;
|
|
} else {
|
|
motor[rightMotor] = 0;
|
|
}
|
|
|
|
// Raise, lower or do not move arm
|
|
if(vexRT[Btn7U] == 1) //If button 5U is pressed...
|
|
{
|
|
motor[armMotor] = 32; //...raise the arm.
|
|
}
|
|
else if(vexRT[Btn7D] == 1) //Else, if button 5D is pressed...
|
|
{
|
|
motor[armMotor] = -32; //...lower the arm.
|
|
}
|
|
else //Else (neither button is pressed)...
|
|
{
|
|
motor[armMotor] = 0; //...stop the arm.
|
|
}
|
|
|
|
// Open, close or do not more claw
|
|
if(vexRT[Btn7L] == 1) //If Button 6U is pressed...
|
|
{
|
|
motor[clawMotor] = 64; //...close the gripper.
|
|
}
|
|
else if(vexRT[Btn7R] == 1) //Else, if button 6D is pressed...
|
|
{
|
|
motor[clawMotor] = -32; //...open the gripper.
|
|
}
|
|
else //Else (neither button is pressed)...
|
|
{
|
|
motor[clawMotor] = 0; //...stop the gripper.
|
|
}
|
|
}
|
|
|
|
// Once we've detected a button press, run the autonomous function block.
|
|
runAutonomous();
|
|
}
|