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(UART_Usage, UART1, uartUserControl, baudRate1200, IOPins, None, None)
#pragma config(Motor, port9, armMotor, tmotorServoContinuousRotation, openLoop, reversed)
@ -65,7 +65,7 @@ enum AutonomousState {
LeftOuter,
RightInner,
RightOuter
}
};
int recvdCodes = 0;
AutonomousState autonomousState = Disabled;
@ -87,7 +87,7 @@ task main() {
while (true) {
// Toggle autonomous mode
if (vexRT[Btn8U]) {
if (vexRT[Btn8R]) {
clearTimer(T1);
autonomousState = RightOuter;
} else if (vexRT[Btn8L]) {
@ -96,7 +96,7 @@ task main() {
} else if (vexRT[Btn7R]) {
clearTimer(T1);
autonomousState = LeftInner;
} else if (vexRT[Btn7U]) {
} else if (vexRT[Btn7L]) {
clearTimer(T1);
autonomousState = LeftOuter;
} else if (vexRT[Btn8D]) {
@ -107,39 +107,59 @@ task main() {
if (autonomousState == RightInner) {
// ANCHOR Right inner
if (time1[T1] >= 1000 && floorColor == Black) {
motor[leftMotor] = 50;
motor[rightMotor] = 25;
} else if (time1[T1] < 1000 && floorColor == Black) {
motor[leftMotor] = 45;
motor[rightMotor] = 30;
} else if (floorColor == White) {
motor[leftMotor] = -40;
motor[leftMotor] = 65;
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) {
// 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) {
// ANCHOR Left inner
if (time1[T1] >= 1000 && floorColor == Black) {
motor[leftMotor] = 25;
motor[rightMotor] = 50;
} else if (time1[T1] < 1000 && floorColor == Black) {
motor[leftMotor] = 30;
motor[rightMotor] = 45;
} else if (floorColor == White) {
motor[rightMotor] = 65;
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) {
// 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 Drive control
else {
// Store joystick axises in variables to allow modification
int verticalAxis = vexRT[Ch3];
int horizontalAxis = vexRT[Ch4];
int armAxis = vexRT[Ch2];
float verticalAxis = vexRT[Ch3];
float horizontalAxis = vexRT[Ch4];
float armAxis = vexRT[Ch2];
// 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
@ -152,10 +172,10 @@ task main() {
// precision for specific tasks
// ANCHOR Slow/fast mode
if (vexRT[Btn6U]) {
verticalAxis = verticalAxis / 2;
horizontalAxis = horizontalAxis / 2;
verticalAxis = verticalAxis * 0.70;
horizontalAxis = horizontalAxis * 0.70;
if (armAxis > 0) { // If putting arm up
armAxis = armAxis * 0.95;
armAxis = armAxis / 3;
} else {
armAxis = armAxis / 4;
}
@ -178,14 +198,14 @@ task main() {
}
// !SECTION Drive control
sleep(3);
sleep(1);
}
}
task sendTask() {
while (true) {
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
}
}
// !SECTION Function/task definitions
// !SECTION Function/task definitions