Cedar-Robotics/BEST-Robotics-2023
1
0
Fork 0
Code Issues 2 Pull requests Projects 1 Releases Packages Wiki Activity

changes question mark emoji

Browse source
This commit is contained in:
Tyler Beckman 2023-11-10 17:23:44 -07:00
parent 8a1b7cc8be
commit ca9d118309
No known key found for this signature in database
1 changed files with 50 additions and 30 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

80
main.c
View file

@ -1,4 +1,4 @@
#pragma config(Motor, port2, leftMotor, tmotorServoContinuousRotation, openLoop) #pragma config(Motor, port2, leftMotor, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor, port3, rightMotor, tmotorServoContinuousRotation, openLoop, reversed) #pragma config(Motor, port3, rightMotor, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(UART_Usage, UART1, uartUserControl, baudRate1200, IOPins, None, None) #pragma config(UART_Usage, UART1, uartUserControl, baudRate1200, IOPins, None, None)
#pragma config(Motor, port9, armMotor, tmotorServoContinuousRotation, openLoop, reversed) #pragma config(Motor, port9, armMotor, tmotorServoContinuousRotation, openLoop, reversed)
@ -65,7 +65,7 @@ enum AutonomousState {
LeftOuter, LeftOuter,
RightInner, RightInner,
RightOuter RightOuter
} };
int recvdCodes = 0; int recvdCodes = 0;
AutonomousState autonomousState = Disabled; AutonomousState autonomousState = Disabled;
@ -87,7 +87,7 @@ task main() {
while (true) { while (true) {
// Toggle autonomous mode // Toggle autonomous mode
if (vexRT[Btn8U]) { if (vexRT[Btn8R]) {
clearTimer(T1); clearTimer(T1);
autonomousState = RightOuter; autonomousState = RightOuter;
} else if (vexRT[Btn8L]) { } else if (vexRT[Btn8L]) {
@ -96,7 +96,7 @@ task main() {
} else if (vexRT[Btn7R]) { } else if (vexRT[Btn7R]) {
clearTimer(T1); clearTimer(T1);
autonomousState = LeftInner; autonomousState = LeftInner;
} else if (vexRT[Btn7U]) { } else if (vexRT[Btn7L]) {
clearTimer(T1); clearTimer(T1);
autonomousState = LeftOuter; autonomousState = LeftOuter;
} else if (vexRT[Btn8D]) { } else if (vexRT[Btn8D]) {
@ -107,39 +107,59 @@ task main() {
if (autonomousState == RightInner) { if (autonomousState == RightInner) {
// ANCHOR Right inner // ANCHOR Right inner
if (time1[T1] >= 1000 && floorColor == Black) { if (time1[T1] >= 1000 && floorColor == Black) {
motor[leftMotor] = 50; motor[leftMotor] = 65;
motor[rightMotor] = 25;
} else if (time1[T1] < 1000 && floorColor == Black) {
motor[leftMotor] = 45;
motor[rightMotor] = 30;
} else if (floorColor == White) {
motor[leftMotor] = -40;
motor[rightMotor] = 40; motor[rightMotor] = 40;
} else if (time1[T1] < 1000 && floorColor == Black) {
motor[leftMotor] = 50;
motor[rightMotor] = 35;
} else if (floorColor == White) {
motor[leftMotor] = -45;
motor[rightMotor] = 45;
} }
} else if (autonomousState == RightOuter) { } else if (autonomousState == RightOuter) {
// TODO Right outer // ANCHOR Right outer
if (time1[T1] >= 1000 && floorColor == Black) {
motor[leftMotor] = 30;
motor[rightMotor] = 65;
} else if (time1[T1] < 1000 && floorColor == Black) {
motor[leftMotor] = 40;
motor[rightMotor] = 65;
} else if (floorColor == White) {
motor[leftMotor] = 45;
motor[rightMotor] = -45;
}
} else if (autonomousState == LeftInner) { } else if (autonomousState == LeftInner) {
// ANCHOR Left inner // ANCHOR Left inner
if (time1[T1] >= 1000 && floorColor == Black) { if (time1[T1] >= 1000 && floorColor == Black) {
motor[leftMotor] = 25; motor[rightMotor] = 65;
motor[rightMotor] = 50;
} else if (time1[T1] < 1000 && floorColor == Black) {
motor[leftMotor] = 30;
motor[rightMotor] = 45;
} else if (floorColor == White) {
motor[leftMotor] = 40; motor[leftMotor] = 40;
motor[rightMotor] = -40; } else if (time1[T1] < 1000 && floorColor == Black) {
motor[rightMotor] = 50;
motor[leftMotor] = 35;
} else if (floorColor == White) {
motor[rightMotor] = -45;
motor[leftMotor] = 45;
} }
} else if (autonomousState == LeftOuter) { } else if (autonomousState == LeftOuter) {
// TODO Left outer // ANCHOR Left outer
} if (time1[T1] >= 1000 && floorColor == Black) {
motor[rightMotor] = 30;
motor[leftMotor] = 65;
} else if (time1[T1] < 1000 && floorColor == Black) {
motor[rightMotor] = 40;
motor[leftMotor] = 65;
} else if (floorColor == White) {
motor[rightMotor] = 45;
motor[leftMotor] = -45;
}
}
// !SECTION IR motor control // !SECTION IR motor control
// SECTION Drive control // SECTION Drive control
else { else {
// Store joystick axises in variables to allow modification // Store joystick axises in variables to allow modification
int verticalAxis = vexRT[Ch3]; float verticalAxis = vexRT[Ch3];
int horizontalAxis = vexRT[Ch4]; float horizontalAxis = vexRT[Ch4];
int armAxis = vexRT[Ch2]; float armAxis = vexRT[Ch2];
// Check if the joystick is pushed enough, if it isn't a significant value, // Check if the joystick is pushed enough, if it isn't a significant value,
// then act as if it was never pushed at all, by setting the value to 0 // then act as if it was never pushed at all, by setting the value to 0
@ -152,10 +172,10 @@ task main() {
// precision for specific tasks // precision for specific tasks
// ANCHOR Slow/fast mode // ANCHOR Slow/fast mode
if (vexRT[Btn6U]) { if (vexRT[Btn6U]) {
verticalAxis = verticalAxis / 2; verticalAxis = verticalAxis * 0.70;
horizontalAxis = horizontalAxis / 2; horizontalAxis = horizontalAxis * 0.70;
if (armAxis > 0) { // If putting arm up if (armAxis > 0) { // If putting arm up
armAxis = armAxis * 0.95; armAxis = armAxis / 3;
} else { } else {
armAxis = armAxis / 4; armAxis = armAxis / 4;
} }
@ -178,14 +198,14 @@ task main() {
} }
// !SECTION Drive control // !SECTION Drive control
sleep(3); sleep(1);
} }
} }
task sendTask() { task sendTask() {
while (true) { while (true) {
sendChar(UART1, 17); sendChar(UART1, 17);
sleep(100); sleep(25);
} }
} }
@ -222,4 +242,4 @@ task recvTask()
sleep(20); // Free up cycle time, so other tasks are able to run consistently sleep(20); // Free up cycle time, so other tasks are able to run consistently
} }
} }
// !SECTION Function/task definitions // !SECTION Function/task definitions