voidf1sh/etc244
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
Skylar Grant 2023-10-03 11:19:26 -07:00
commit 6587d5a8ea
1 changed files with 309 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

309
Labs.c Normal file
View File

@ -0,0 +1,309 @@
/* Progressive Development Code
* for ETC 244 VeX Robotics Lab
* Instructor: Bill Dolan
* Fall 2023-2024
*
* Developed by Skylar Grant
*/
// Configuration created by ROBOTC
#pragma config(Sensor, in3, armPos, sensorPotentiometer)
#pragma config(Sensor, in4, lightRight, sensorReflection)
#pragma config(Sensor, in5, lightLeft, sensorReflection)
#pragma config(Sensor, in6, lineLeft, sensorLineFollower)
#pragma config(Sensor, in7, lineCenter, sensorLineFollower)
#pragma config(Sensor, in8, lineRight, sensorLineFollower)
#pragma config(Sensor, dgtl1, leftRotary1, sensorQuadEncoder)
#pragma config(Sensor, dgtl3, rightRotary1, sensorQuadEncoder)
#pragma config(Sensor, dgtl5, frontBumper, sensorDigitalIn)
#pragma config(Sensor, dgtl6, rearBumper, sensorDigitalIn)
#pragma config(Sensor, dgtl7, startBumper, sensorDigitalIn)
#pragma config(Sensor, dgtl8, sonarIn, sensorSONAR_inch)
#pragma config(Sensor, dgtl10, armLimit, sensorDigitalIn)
#pragma config(Motor, port1, rightMotor, tmotorVex393_HBridge, openLoop, reversed, driveRight, encoderPort, dgtl3)
#pragma config(Motor, port3, clawMotor, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port4, armMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port10, leftMotor, tmotorVex393_HBridge, openLoop, reversed, driveLeft, encoderPort, dgtl1)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/* 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;
}
/* 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(16);
}
/* Wait for the start button to be pressed, then run autonomous code.
*
* Arguments: None
*/
void waitForStart() {
while (SensorValue[startBumper]==1) {
wait1Msec(500);
}
// Once we've detected a button press, run the autonomous function block.
runAutonomous();
// 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() {
waitForStart();
}