Shuto Naruse
Eurobot2012_Secondary
Fork of
Eurobot_2012_Secondary
by 
Shuto Naruse
Diff: main.cpp
- Revision:
- 1:cc2a9eb0bd55
- Parent:
- 0:fbfafa6bf5f9
--- a/main.cpp Fri Apr 20 21:32:24 2012 +0000
+++ b/main.cpp Wed Oct 17 22:25:31 2012 +0000
@@ -7,91 +7,260 @@
#include "math.h"
#include "system.h"
#include "geometryfuncs.h"
+#include "motion.h"
+#include "ai.h"
+#include "ui.h"
//#include <iostream>
//Interface declaration
-//I2C i2c(p28, p27); // sda, scl
-TSI2C i2c(p28, p27);
Serial pc(USBTX, USBRX); // tx, rx
-Serial IRturret(p13, p14);
-DigitalOut OLED1(LED1);
-DigitalOut OLED2(LED2);
-DigitalOut OLED3(LED3);
-DigitalOut OLED4(LED4);
-
-Motors motors(i2c);
-Kalman kalman(motors);
-
-float targetX = 1000, targetY = 1000, targetTheta = 0;
+bool Colour = 1; // 1 for red, 0 for blue
+pos beaconpos[] = {{3000, 1000},{0,0}, {0,2000}}; //predefined red start
-// bytes packing/unpacking for IR turret serial comm
-union IRValue_t {
- float IR_floats[3];
- int IR_ints[3];
- unsigned char IR_chars[12];
-} IRValues;
+DigitalIn StartTrig(p11);
+DigitalIn ColourToggle(p19); //high for red, low for blue(purple)
+Ticker StopTicker;
-char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC};
-int Alignment_ptr = 0;
-bool data_flag = false;
-int buff_pointer = 0;
-bool angleInit = false;
-float angleOffset = 0;
-
-void vIRValueISR (void);
-void vKalmanInit(void);
-
-//TODO mutex on kalman state, and on motor commands (i.e. on the i2c bus)
-//NOTE! Recieving data with RF12B now DISABLED due to interferance with rtos!
+TSI2C i2c(p28, p27);
+Motors motors(i2c);
+UI ui;
+Kalman kalman(motors,ui,p10,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9);
+AI ai;
+Motion motion(motors, ai, kalman);
void vMotorThread(void const *argument);
void vPrintState(void const *argument);
-void ai_thread (void const *argument);
void motion_thread(void const *argument);
-
-
-float getAngle (float x, float y);
-void getIRValue(void const *argument);
+void vStop (void);
-// Thread pointers
-Thread *AI_Thread_Ptr;
-Thread *Motion_Thread_Ptr;
-
-Mutex targetlock;
-bool flag_terminate = false;
+//bool flag_terminate = false;
float temp = 0;
//Main loop
int main() {
+ai.flag_manOverride = true;
+ // no motor motions till we pull the trig
+ ai.flag_motorStop = true;
+
+ Colour = ColourToggle;
+ OLED3 = Colour;
+ // re-defines beacon positions by the toggle switch
+ kalman.statelock.lock();
+ if (true) {
+ beaconpos[0].x = 3000;
+ beaconpos[0].y = 1000;
+ beaconpos[1].x = 0;
+ beaconpos[1].y = 0;
+ beaconpos[2].x = 0;
+ beaconpos[2].y = 2000;
+ //beaconpos[] = {{3000, 1000},{0,0}, {0,2000}};
+ } else {
+ beaconpos[0].x = 0;
+ beaconpos[0].y = 1000;
+ beaconpos[1].x = 3000;
+ beaconpos[1].y = 0;
+ beaconpos[2].x = 3000;
+ beaconpos[2].y = 2000;
+ //beaconpos[] = {{0, 1000},{3000,0}, {3000,2000}};
+ }
+ kalman.statelock.unlock();
pc.baud(115200);
- IRturret.baud(115200);
- IRturret.format(8,Serial::Odd,1);
- IRturret.attach(&vIRValueISR,Serial::RxIrq);
- vKalmanInit();
+
+
+ //Init kalman, this should be done in the mid of the arena before the game starts
+ kalman.KalmanInit();
//Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
- Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
-
- Thread thr_AI(ai_thread,NULL,osPriorityNormal,1024);
- Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024);
- AI_Thread_Ptr = &thr_AI;
- Motion_Thread_Ptr = &thr_motion;
+ //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
//measure cpu usage. output updated once per second to symbol cpupercent
//Thread mCPUthread(measureCPUidle, NULL, osPriorityIdle, 1024); //check if stack overflow with such a small staack
+ pc.printf("We got to main! ;D\r\n");
- pc.printf("We got to main! ;D\r\n");
+ if (Colour)
+ printf("I'm in Red \n\r");
+ else
+ printf("I'm in Blue \n\r");
//REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
while (1) {
+
+ osThreadSetPriority (osThreadGetId(), osPriorityIdle);
+
+ Timer timer;
+ ui.regid(10, 1);
+
+ while (1) {
+ timer.reset();
+ timer.start();
+ nopwait(1000);
+
+ ui.updateval(10, timer.read_us());
+ }
+
// do nothing
+ //Thread::wait(osWaitForever);
+ }
+}
+
+void AI::ai_thread () {
+ //Thread::wait(1000);
+
+ printf("Waiting for the trigger pull ....\r\n");
+
+ // wait for the start triger
+ while (!StartTrig) {
+ Thread::wait(10);
+ };
+
+
+ //printf("GO! \r\n");
+ kalman.KalmanReset();
+ Thread::wait(100);
+
+
+ // attach a 87 seconds stop timer
+ StopTicker.attach(&vStop, 87);
+
+
+
+
+ // starts motors
+ ai.flag_motorStop = false;
+
+// ai.flag_manOverride = true;
+// motors.setSpeed(20);
+// Thread::wait(5000);
+ ai.flag_manOverride = false;
+
+
+
+// if (Colour) {
+ // strat 1 RED ==================================
+
+ // run out to black line
+ settarget(640,300,PI/2,true,Colour, MOVE_SPEED);
+ Thread::signal_wait(0x01);
+
+/*
+//prep to move block
+settarget(640, 860,PI,true,Colour, MOVE_SPEED);
+Thread::signal_wait(0x01);
+
+//move block to ship
+settarget(173, 860,PI,true,Colour, MOVE_SPEED);
+Thread::signal_wait(0x01);
+
+//back out
+settarget(640,860,PI/2,false,Colour, MOVE_SPEED);
+Thread::signal_wait(0x01);
+*/
+
+ // intermediate
+ settarget(640,1600,-PI/2,true,Colour, MOVE_SPEED);
+ Thread::signal_wait(0x01);
+
+ //push first button
+ settarget(640,2200,-PI/2,false,Colour, MOVE_SPEED);
+ //Thread::signal_wait(0x01,10000);
+ Thread::wait(5000);
+
+
+//back out from button
+ settarget(640,1500,-PI/2,true,Colour, MOVE_SPEED);
+ Thread::signal_wait(0x01);
+
+ // go get middle bullion
+ settarget(1500,1320,0,true,Colour, MOVE_SPEED);
+ Thread::signal_wait(0x01);
+
+
+ // go to second button
+ settarget(1920,1700,-PI/2,true,Colour, MOVE_SPEED);
+ Thread::signal_wait(0x01);
+
+
+ // push button
+ settarget(1920,2200,-PI/2,false,Colour, MOVE_SPEED);
+ Thread::wait(5000);
+
+ // back off
+ settarget(1920,1700,-PI/2,true,Colour, MOVE_SPEED);
+ Thread::signal_wait(0x01);
+
+ // go somewhere
+ settarget(2400,1000,-PI/2,true,Colour, MOVE_SPEED);
+ Thread::signal_wait(0x01);
+
+//go to opponents captain area
+settarget(2200,280,PI,true,Colour, MOVE_SPEED);
+Thread::signal_wait(0x01);
+
+//back to own captain area
+settarget(260,275,PI,true,Colour, MOVE_SPEED);
+Thread::signal_wait(0x01);
+
+
+
+//back to black line
+settarget(640,300,0,false,Colour, MOVE_SPEED);
+Thread::signal_wait(0x01);
+
+
+//grab CD
+settarget(1000,525,PI,true,Colour, MOVE_SPEED);
+Thread::signal_wait(0x01);
+
+
+//back to own captain area
+settarget(260,275,PI,true,Colour, MOVE_SPEED);
+Thread::signal_wait(0x01);
+
+
+
+//to opponent's totem
+settarget(1750,850,0,false,Colour, MOVE_SPEED);
+Thread::signal_wait(0x01);
+
+
+
+//collect their CDs
+settarget(2000,750,PI,true,Colour, MOVE_SPEED);
+Thread::signal_wait(0x01);
+
+
+
+//collect more
+settarget(2190,1155,0,true,Colour, MOVE_SPEED);
+Thread::signal_wait(0x01);
+
+//go to opponents captain area
+settarget(2200,280,PI,true,Colour, MOVE_SPEED);
+Thread::signal_wait(0x01);
+
+
+
+//go back to ship
+settarget(210,900,PI,true,Colour, MOVE_SPEED);
+Thread::signal_wait(0x01);
+
+
+
+//END
+
+ // terminate thread, stopps motors permanently
+ ai.flag_terminate = true;
+ while (true) {
Thread::wait(osWaitForever);
}
+
+
+ // end of strat 1 ===========================
}
@@ -118,7 +287,6 @@
}
-
void vPrintState(void const *argument) {
float state[3];
float SonarMeasures[3];
@@ -138,242 +306,18 @@
IRMeasures[2] = kalman.IRMeasures[2];
kalman.statelock.unlock();
pc.printf("\r\n");
- pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]);
+ pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]*180/PI);
pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]);
pc.printf("IR : %0.4f %0.4f %0.4f \r\n",IRMeasures[0]*180/PI,IRMeasures[1]*180/PI,IRMeasures[2]*180/PI);
- pc.printf("Angle_Offset: %0.4f \r\n",angleOffset*180/PI);
Thread::wait(100);
}
}
-
-// AI thread ------------------------------------
-void ai_thread (void const *argument) {
- // goes to the mid
- Thread::signal_wait(0x01);
- targetlock.lock();
- targetX = 1500;
- targetY = 1000;
- targetTheta = PI/2;
- targetlock.unlock();
-
- // left roll
- Thread::signal_wait(0x01);
- targetlock.lock();
- targetX = 500;
- targetY = 1700;
- targetTheta = PI/2;
- targetlock.unlock();
-
- // mid
- Thread::signal_wait(0x01);
- targetlock.lock();
- targetX = 1500;
- targetY = 1000;
- targetTheta = PI/2;
- targetlock.unlock();
-
- // map
- Thread::signal_wait(0x01);
- targetlock.lock();
- targetX = 1500;
- targetY = 1700;
- targetTheta = PI/2;
- targetlock.unlock();
-
- // mid
- Thread::signal_wait(0x01);
- targetlock.lock();
- targetX = 1500;
- targetY = 1000;
- targetTheta = -PI/2;
- targetlock.unlock();
-
- // home
- Thread::signal_wait(0x01);
- targetlock.lock();
- targetX = 500;
- targetY = 500;
- targetTheta = 0;
- targetlock.unlock();
-
- Thread::signal_wait(0x01);
- flag_terminate = true;
- //OLED3 = true;
-
- while (true) {
- Thread::wait(osWaitForever);
- }
+void vStop (void) {
+// while (true) {
+ motors.coastStop();
+ ai.flag_motorStop = true;
+ // terminate thread, stopps motors permanently
+ ai.flag_terminate = true;
+// };
}
-
-// motion control thread ------------------------
-void motion_thread(void const *argument) {
- motors.resetEncoders();
- motors.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2);
- Thread::wait(1000);
- motors.stop();
- (*AI_Thread_Ptr).signal_set(0x01);
-
-
-
- float currX, currY,currTheta;
- float speedL,speedR;
- float diffDir,diffSpeed;
- float lastdiffSpeed = 0;
-
- while (1) {
- if (flag_terminate) {
- terminate();
- }
-
- // get kalman localization estimate ------------------------
- kalman.statelock.lock();
- currX = kalman.X(0)*1000.0f;
- currY = kalman.X(1)*1000.0f;
- currTheta = kalman.X(2);
- kalman.statelock.unlock();
-
-
- // check if target reached ----------------------------------
- if ( ( abs(currX - targetX) < POSITION_TOR )
- &&( abs(currY - targetY) < POSITION_TOR )
- ) {
-
- diffDir = rectifyAng(currTheta - targetTheta);
- diffSpeed = diffDir / PI;
-
- if (abs(diffDir) > ANGLE_TOR) {
- if (abs(diffSpeed) < 0.5) {
- diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
- }
- motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED));
-
-
- } else {
- motors.stop();
- Thread::wait(4000);
- (*AI_Thread_Ptr).signal_set(0x01);
- }
- }
-
- // adjust motion to reach target ----------------------------
- else {
-
- // calc direction to target
- float targetDir = atan2(targetY - currY, targetX - currX);
-
-
- diffDir = rectifyAng(currTheta - targetDir);
- diffSpeed = diffDir / PI;
-
- if (abs(diffDir) > ANGLE_TOR*2) {
- if (abs(diffSpeed) < 0.5) {
- diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
- }
- motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED));
- } else {
-
-
- if (abs(diffSpeed-lastdiffSpeed) > MAX_STEP_RATIO ) {
- if (diffSpeed-lastdiffSpeed >= 0) {
- diffSpeed = lastdiffSpeed + MAX_STEP_RATIO;
- } else {
- diffSpeed = lastdiffSpeed - MAX_STEP_RATIO;
- }
- }
- lastdiffSpeed = diffSpeed;
-
- // calculte the motor speeds
- if (diffSpeed <= 0) {
- speedL = (1-2*abs(diffSpeed))*MOVE_SPEED;
- speedR = MOVE_SPEED;
-
- } else {
- speedR = (1-2*abs(diffSpeed))*MOVE_SPEED;
- speedL = MOVE_SPEED;
- }
-
- motors.setSpeed( int(speedL), int(speedR));
- }
- }
-
- // wait
- Thread::wait(MOTION_UPDATE_PERIOD);
- }
-}
-
-void vIRValueISR (void) {
-
- OLED3 = !OLED3;
- // A workaround for mbed UART ISR bug
- // Clear the RBR flag to make sure the interrupt doesn't loop
- // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14.
- // UART0 for USB UART
- unsigned char RBR = LPC_UART1->RBR;
-
-
- if (!data_flag) { // look for alignment bytes
- if (RBR == Alignment_char[Alignment_ptr]) {
- Alignment_ptr ++;
- }
- if (Alignment_ptr >= 4) {
- Alignment_ptr = 0;
- data_flag = true; // set the dataflag
- }
- } else { // fetch data bytes
- IRValues.IR_chars[buff_pointer] = RBR;
- buff_pointer ++;
- if (buff_pointer >= 12) {
- buff_pointer = 0;
- data_flag = false; // dessert the dataflag
- if (angleInit) {
- kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1]+angleOffset),IRValues.IR_floats[2]);
- } else {
- kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]);
- }
- }
-
- }
-}
-
-void vKalmanInit(void) {
- float SonarMeasures[3];
- float IRMeasures[3];
- int beacon_cnt = 0;
- wait(1);
- IRturret.attach(NULL,Serial::RxIrq);
- kalman.statelock.lock();
- SonarMeasures[0] = kalman.SonarMeasures[0]*1000.0f;
- SonarMeasures[1] = kalman.SonarMeasures[1]*1000.0f;
- SonarMeasures[2] = kalman.SonarMeasures[2]*1000.0f;
- IRMeasures[0] = kalman.IRMeasures[0];
- IRMeasures[1] = kalman.IRMeasures[1];
- IRMeasures[2] = kalman.IRMeasures[2];
- kalman.statelock.unlock();
- //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasures[0]*180/PI, IRMeasures[1]*180/PI, IRMeasures[2]*180/PI);
- float d = beaconpos[2].y - beaconpos[1].y;
- float i = beaconpos[0].y - beaconpos[1].y;
- float j = beaconpos[0].x - beaconpos[1].x;
- float y_coor = (SonarMeasures[1]*SonarMeasures[1]- SonarMeasures[2]*SonarMeasures[2] + d*d) / (2*d);
- float x_coor = (SonarMeasures[1]*SonarMeasures[1] - SonarMeasures[0]*SonarMeasures[0] + i*i + j*j)/(2*j) - i*y_coor/j;
- angleOffset = 0;
- for (int i = 0; i < 3; i++) {
- float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
- if (IRMeasures[i] != 0){
- beacon_cnt ++;
- float angle_temp = angle_est - IRMeasures[i];
- angle_temp -= (floor(angle_temp/(2*PI)))*2*PI;
- angleOffset += angle_temp;
- }
- }
- angleOffset = angleOffset/float(beacon_cnt);
- //printf("\n\r");
- angleInit = true;
- kalman.statelock.lock();
- kalman.X(0) = x_coor/1000.0f;
- kalman.X(1) = y_coor/1000.0f;
- kalman.X(2) = 0;
- kalman.statelock.unlock();
- //printf("x: %0.4f, y: %0.4f, offset: %0.4f \n\r", x_coor, y_coor, angleOffset*180/PI);
- IRturret.attach(&vIRValueISR,Serial::RxIrq);
-}
\ No newline at end of file