From 16883bc123c8e55a0945bd89b400d70ccfc2ef1f Mon Sep 17 00:00:00 2001 From: Ty Date: Wed, 25 Oct 2023 20:57:47 -0600 Subject: [PATCH] Upload code finally --- .vscode/settings.json | 7 ++ README.md | 10 ++ irtest.c | 115 +++++++++++++++++++++ main.c | 225 ++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 357 insertions(+) create mode 100644 .vscode/settings.json create mode 100644 README.md create mode 100644 irtest.c create mode 100644 main.c diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..5c6ee21 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,7 @@ +{ + "C_Cpp.default.compilerPath": "", + "C_Cpp.errorSquiggles": "disabled", + "liveshare.publishWorkspaceInfo": true, + "files.autoSave": "onWindowChange", + "commentAnchors.tagHighlights.enabled": true +} \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..603bc78 --- /dev/null +++ b/README.md @@ -0,0 +1,10 @@ +# BEST-robotics-2023 + +This is the robot code for DSST Cedar's robot in the 2023 BEST robotics competition. + +## Port mappings + +| Port | Motor | +| ---- | ----------------- | +| 2 | Left drive motor | +| 3 | Right drive motor | \ No newline at end of file diff --git a/irtest.c b/irtest.c new file mode 100644 index 0000000..32f6aad --- /dev/null +++ b/irtest.c @@ -0,0 +1,115 @@ +#pragma config(UART_Usage, UART1, uartUserControl, baudRate1200, IOPins, None, None) + +// custom baud rate set code +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; +} + +enum IRState { + Black = 0, + White = 1 +}; + +task detectTask(); +task recvTask(); + +int charr = -1; +int recvdCodes = 0; +IRState floorColor = Black; + +// ANCHOR Robot code +task main() +{ + // Setup the IR baud rate + setBaud(UART1, 600); + + startTask(detectTask); + startTask(recvTask); + // Loop forever + while (true) + { + if (vexRT[Btn8L]) + { + sendChar(UART1, 17); + + sleep(100); + } + } +} + +task detectTask() +{ + int cycles = 0; + int cached = recvdCodes; + + while (true) { + if (cycles % 5 == 0) { + if (cached == recvdCodes) { + floorColor = Black; + } else { + floorColor = White; + } + } + cached = recvdCodes; + sleep(100); + } +} + +task recvTask() +{ + while (true) + { + charr = getChar(UART1); + if (charr != -1) + { + recvdCodes = recvdCodes + 1; + } + + sleep(20); // Free up cycle time + } +} diff --git a/main.c b/main.c new file mode 100644 index 0000000..a9ab7bb --- /dev/null +++ b/main.c @@ -0,0 +1,225 @@ +#pragma config(Motor, port2, leftMotor, tmotorServoContinuousRotation, openLoop) +#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 +} + +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 + if (vexRT[Btn8U]) { + clearTimer(T1); + autonomousState = RightOuter; + } else if (vexRT[Btn8L]) { + clearTimer(T1); + autonomousState = RightInner; + } else if (vexRT[Btn7R]) { + clearTimer(T1); + autonomousState = LeftInner; + } else if (vexRT[Btn7U]) { + 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) { + 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[rightMotor] = 40; + } + } else if (autonomousState == RightOuter) { + // TODO Right outer + } 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[leftMotor] = 40; + motor[rightMotor] = -40; + } + } else if (autonomousState == LeftOuter) { + // TODO Left outer + } + // !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]; + + // 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]) { + verticalAxis = verticalAxis / 2; + horizontalAxis = horizontalAxis / 2; + if (armAxis > 0) { // If putting arm up + armAxis = armAxis * 0.95; + } 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 + + sleep(3); + } +} + +task sendTask() { + while (true) { + sendChar(UART1, 17); + sleep(100); + } +} + +// 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 + } +} +// !SECTION Function/task definitions \ No newline at end of file