From 302ad028cf51d12ec4f28334125628141adcbb1e Mon Sep 17 00:00:00 2001 From: Skylar Grant Date: Fri, 6 Oct 2023 07:30:17 -0400 Subject: [PATCH] Added arm safety check --- Labs.c | 232 +++++++++++++++++++++++++++++++++++---------------------- 1 file changed, 143 insertions(+), 89 deletions(-) diff --git a/Labs.c b/Labs.c index 9469701..2be59ce 100644 --- a/Labs.c +++ b/Labs.c @@ -45,6 +45,41 @@ void setMotors(int leftSpeed, int rightSpeed) { motor[rightMotor] = rightSpeed; } +//==================================================================================// + +/* Arm safety checker, verifies potentiometer position and limit switch status + * of the arm to disable the motors automatically. + * + * Arguments: + * - direction: single character + * - "U" | "D": Up or Down respectively + * - Determines which direction the arm wants to move, otherwise the arm would always + * be stuck in a limit position. + * + * Returns: + * - integer: Status, 0 means arm is in a blocked position, 1 means the arm can move + * + * Notes: + * None. +*/ +int checkArm(char *direction) { + if (direction == "U") { // The arm wants to go up + // So we check the potentiometer + if (SensorValue[armPos] < 1800) { // 1800 limit / 2300 max + return 1; // The arm can move up more + } else { + return 0; // The arm is at the upper limit, do not move it up more + } + } else if (direction == "D") { // The arm wants to go down + // So we check the limit switch + if (SensorValue[armLimit] == 1) { // The limit switch isn't being pressed + return 1; // The arm can move down more + } else { // The switch is being pressed down + return 0; // The arm cannot be moved down more + } + } +} + /* Quickly set the arm motor speed * * Arguments: @@ -55,8 +90,24 @@ void setMotors(int leftSpeed, int rightSpeed) { * 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; + // Check the arm safety + int safe = 0; + if (speed > 0) { + safe = checkArm("U"); + } else if (speed < 0) { + safe = checkArm("D"); + } else { + safe = 0; + } + + if (safe == 1) { + // Set the arm motor speed + motor[armMotor] = speed; + } else { + // Turn off the motor + motor[armMotor] = 0; + } + } /* Quickly set the claw motor speed @@ -219,6 +270,27 @@ void moveArm(int speed, int timeMs) { motor[armMotor] = 0; } +//==================================================================================// + +/* Move the bot's claw motor + * + * Arguments: + * - speed: signed integer. -127 to 127 + * - <0: Down + * - >0: Up + * - Speed to drive the claw motor + * - timeMs: signed integer + * - Time to wait before stopping the claw, in milliseconds +*/ +void moveClaw(int speed, int timeMs) { + // Turn on the motor at the set speed + motor[clawMotor] = speed; + // Wait for the set time + wait1Msec(timeMs); + // Turn the motor off + motor[clawMotor] = 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 @@ -308,104 +380,86 @@ void clawRelease() { * * Arguments: None * Notes: TODO: Find a way to recall main() so this can be rerun without rebooting/recompiling + * + * Lab Description: + * Have arm motor + * - Run autonomous with start button + * - Arm up 1000ms + * - Arm down 1000ms + * - Claw release 1000ms + * - Claw grab 1000ms + * - Repeat @50% speed */ 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); + moveArm(127, 1000); + moveArm(-127, 1000); + moveClaw(127, 1000); + moveClaw(-127, 1000); + moveArm(64, 1000); + moveArm(-64, 1000); + moveClaw(64, 1000); + moveClaw(-64, 1000); } -//==================================================================================// - -/* 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; + // Loop over this code infinitely, this will allow running the autocode more than once + while (1) { + // As long as the start button isn't pressed (Logical 1) run the code for the controller input + 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(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 7U is pressed... + { + motor[armMotor] = 32; //...raise the arm. + } + else if(vexRT[Btn7D] == 1) //Else, if button 7D 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 7L is pressed... + { + motor[clawMotor] = 64; //...open the gripper. + } + else if(vexRT[Btn7R] == 1) //Else, if button 7R is pressed... + { + motor[clawMotor] = -32; //...close the gripper. + } + else //Else (neither button is pressed)... + { + motor[clawMotor] = 0; //...stop the gripper. + } } - 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(); } - - // Once we've detected a button press, run the autonomous function block. - runAutonomous(); }