#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(); }