Upload code finally
This commit is contained in:
commit
16883bc123
GPG key ID:
2813440C772555A4
4 changed files with 357 additions and 0 deletions
7
.vscode/settings.json
vendored
Normal file
7
.vscode/settings.json
vendored
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
{
|
||||
"C_Cpp.default.compilerPath": "",
|
||||
"C_Cpp.errorSquiggles": "disabled",
|
||||
"liveshare.publishWorkspaceInfo": true,
|
||||
"files.autoSave": "onWindowChange",
|
||||
"commentAnchors.tagHighlights.enabled": true
|
||||
}
|
||||
10
README.md
Normal file
10
README.md
Normal file
|
|
@ -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 |
|
||||
115
irtest.c
Normal file
115
irtest.c
Normal file
|
|
@ -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
|
||||
}
|
||||
}
|
||||
225
main.c
Normal file
225
main.c
Normal file
|
|
@ -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
|
||||
Loading…
Reference in a new issue