BEST-Robotics-2023/main.c

246 lines
6.7 KiB
C
Raw Permalink Normal View History

2023-11-10 17:23:44 -07:00
#pragma config(Motor, port2, leftMotor, tmotorServoContinuousRotation, openLoop, reversed)
2023-10-25 20:57:47 -06:00
#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)
// ANCHOR Baud rate library
typedef unsigned long uint32_t;
typedef unsigned short uint16_t;
typedef struct
{
uint16_t SR;
uint16_t RESERVED0;
uint16_t DR;
uint16_t RESERVED1;
uint16_t BRR;
uint16_t RESERVED2;
uint16_t CR1;
uint16_t RESERVED3;
uint16_t CR2;
uint16_t RESERVED4;
uint16_t CR3;
uint16_t RESERVED5;
uint16_t GTPR;
uint16_t RESERVED6;
} USART_TypeDef;
/* Peripheral memory map */
#define PERIPH_BASE ((unsigned long)0x40000000)
#define APB1PERIPH_BASE PERIPH_BASE
#define USART2_BASE (APB1PERIPH_BASE + 0x4400)
#define USART3_BASE (APB1PERIPH_BASE + 0x4800)
#define USART2 ((USART_TypeDef *)USART2_BASE)
#define USART3 ((USART_TypeDef *)USART3_BASE)
void setBaud(const TUARTs nPort, int baudRate)
{
uint32_t tmpreg = 0x00, apbclock = 0x00;
uint32_t integerdivider = 0x00;
uint32_t fractionaldivider = 0x00;
/* pclk1 - 36MHz */
apbclock = 36000000;
/* Determine the integer part */
integerdivider = ((0x19 * apbclock) / (0x04 * (baudRate)));
tmpreg = (integerdivider / 0x64) << 0x04;
/* Determine the fractional part */
fractionaldivider = integerdivider - (0x64 * (tmpreg >> 0x04));
tmpreg |= ((((fractionaldivider * 0x10) + 0x32) / 0x64)) & 0x0F;
/* Write to USART BRR */
USART_TypeDef *uart = USART2;
if (nPort == UART2)
{
uart = USART3;
}
uart->BRR = (uint16_t)tmpreg;
}
// ANCHOR Variable state definitions
enum IRState {
Black,
White
};
enum AutonomousState {
Disabled,
LeftInner,
LeftOuter,
RightInner,
RightOuter
2023-11-10 17:23:44 -07:00
};
2023-10-25 20:57:47 -06:00
int recvdCodes = 0;
AutonomousState autonomousState = Disabled;
IRState floorColor = Black;
// SECTION Function/task definitions
task sendTask();
task detectTask();
task recvTask();
// Main program loop
task main() {
// Prep IR sensors
setBaud(UART1, 600);
startTask(sendTask);
startTask(detectTask);
startTask(recvTask);
while (true) {
// Toggle autonomous mode
2023-11-10 17:23:44 -07:00
if (vexRT[Btn8R]) {
2023-10-25 20:57:47 -06:00
clearTimer(T1);
autonomousState = RightOuter;
} else if (vexRT[Btn8L]) {
clearTimer(T1);
autonomousState = RightInner;
} else if (vexRT[Btn7R]) {
clearTimer(T1);
autonomousState = LeftInner;
2023-11-10 17:23:44 -07:00
} else if (vexRT[Btn7L]) {
2023-10-25 20:57:47 -06:00
clearTimer(T1);
autonomousState = LeftOuter;
} else if (vexRT[Btn8D]) {
autonomousState = Disabled;
}
// SECTION IR motor control
if (autonomousState == RightInner) {
// ANCHOR Right inner
if (time1[T1] >= 1000 && floorColor == Black) {
2023-11-10 17:23:44 -07:00
motor[leftMotor] = 65;
motor[rightMotor] = 40;
2023-10-25 20:57:47 -06:00
} else if (time1[T1] < 1000 && floorColor == Black) {
2023-11-10 17:23:44 -07:00
motor[leftMotor] = 50;
motor[rightMotor] = 35;
2023-10-25 20:57:47 -06:00
} else if (floorColor == White) {
2023-11-10 17:23:44 -07:00
motor[leftMotor] = -45;
motor[rightMotor] = 45;
2023-10-25 20:57:47 -06:00
}
} else if (autonomousState == RightOuter) {
2023-11-10 17:23:44 -07:00
// 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;
}
2023-10-25 20:57:47 -06:00
} else if (autonomousState == LeftInner) {
// ANCHOR Left inner
if (time1[T1] >= 1000 && floorColor == Black) {
2023-11-10 17:23:44 -07:00
motor[rightMotor] = 65;
motor[leftMotor] = 40;
2023-10-25 20:57:47 -06:00
} else if (time1[T1] < 1000 && floorColor == Black) {
2023-11-10 17:23:44 -07:00
motor[rightMotor] = 50;
motor[leftMotor] = 35;
2023-10-25 20:57:47 -06:00
} else if (floorColor == White) {
2023-11-10 17:23:44 -07:00
motor[rightMotor] = -45;
motor[leftMotor] = 45;
2023-10-25 20:57:47 -06:00
}
} else if (autonomousState == LeftOuter) {
2023-11-10 17:23:44 -07:00
// 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;
}
}
2023-10-25 20:57:47 -06:00
// !SECTION IR motor control
// SECTION Drive control
else {
// Store joystick axises in variables to allow modification
2023-11-10 17:23:44 -07:00
float verticalAxis = vexRT[Ch3];
float horizontalAxis = vexRT[Ch4];
float armAxis = vexRT[Ch2];
2023-10-25 20:57:47 -06:00
// 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
// ANCHOR Deadzones
if (verticalAxis < 15 && verticalAxis > -15) verticalAxis = 0;
if (horizontalAxis < 15 && horizontalAxis > -15) horizontalAxis = 0;
if (armAxis < 15 && armAxis > -15) armAxis = 0;
// Halve the joystick values if the slow mode button is pressed to allow
// precision for specific tasks
// ANCHOR Slow/fast mode
if (vexRT[Btn6U]) {
2023-11-10 17:23:44 -07:00
verticalAxis = verticalAxis * 0.70;
horizontalAxis = horizontalAxis * 0.70;
2023-10-25 20:57:47 -06:00
if (armAxis > 0) { // If putting arm up
2023-11-10 17:23:44 -07:00
armAxis = armAxis / 3;
2023-10-25 20:57:47 -06:00
} else {
armAxis = armAxis / 4;
}
} else if (vexRT[Btn6D]) {
verticalAxis = verticalAxis * 2;
horizontalAxis = horizontalAxis * 2;
armAxis = armAxis * 2;
}
// ANCHOR Set motor speeds
motor[leftMotor] = (verticalAxis + horizontalAxis)/2;
motor[rightMotor] = (verticalAxis - horizontalAxis)/2;
// Check for arm stable button else joystick
if (vexRT[Btn5U]) {
motor[armMotor] = 15;
} else {
motor[armMotor] = armAxis;
}
}
// !SECTION Drive control
2023-11-10 17:23:44 -07:00
sleep(1);
2023-10-25 20:57:47 -06:00
}
}
task sendTask() {
while (true) {
sendChar(UART1, 17);
2023-11-10 17:23:44 -07:00
sleep(25);
2023-10-25 20:57:47 -06:00
}
}
// Make a task with the job of constantly calculating the floor color based on recvd code count
task detectTask()
{
int cached = recvdCodes; // Cache the recvdCodes variable to allow checking for change
while (true) {
// Check if the amount of recvd codes has not changed
if (cached == recvdCodes) { // If it has not changed, the codes were absorbed by black tape
// clearTimer(T2);
floorColor = Black;
} else { // If it has changed, the codes were reflected, so it is seeing white
clearTimer(T1);
floorColor = White;
}
cached = recvdCodes;
sleep(100);
}
}
// Make a task with the job of constantly checking the IR sensor for recvd codes, and storing it
task recvTask()
{
while (true)
{
int charr = getChar(UART1);
if (charr != -1)
{
recvdCodes = recvdCodes + 1;
}
sleep(20); // Free up cycle time, so other tasks are able to run consistently
}
}
2023-11-10 17:23:44 -07:00
// !SECTION Function/task definitions