225 lines
6 KiB
C
225 lines
6 KiB
C
|
#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
|