commit 6587d5a8eaabb5867766bd91d99839553c7c140b
Author: Skylar Grant <sky@vfsh.dev>
Date:   Tue Oct 3 11:19:26 2023 -0700

    Initial commit

diff --git a/Labs.c b/Labs.c
new file mode 100644
index 0000000..0192b87
--- /dev/null
+++ b/Labs.c
@@ -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();
+}