Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Diff: main.cpp
- Revision:
- 2:cffa347bb943
- Parent:
- 1:bbabbd997d21
- Child:
- 4:7b7334441da9
diff -r bbabbd997d21 -r cffa347bb943 main.cpp --- a/main.cpp Fri Apr 20 21:56:15 2012 +0000 +++ b/main.cpp Thu Apr 26 21:02:12 2012 +0000 @@ -7,41 +7,20 @@ #include "math.h" #include "system.h" #include "geometryfuncs.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(p9, p10); - -DigitalOut OLED1(LED1); -DigitalOut OLED2(LED2); -DigitalOut OLED3(LED3); -DigitalOut OLED4(LED4); Motors motors; -Kalman kalman(motors); +UI ui; -float targetX = 1000, targetY = 1000, targetTheta = 0; -// 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; - -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); +Kalman kalman(motors,ui,p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11,p9,p10); +AI ai; //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! @@ -49,42 +28,30 @@ 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); - -// 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() { pc.baud(115200); - IRturret.baud(115200); - IRturret.format(8,Serial::Odd,1); - //IRturret.attach(&vIRValueISR,Serial::RxIrq); - //vKalmanInit(); + //Init kalman + 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; //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"); //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!! @@ -141,80 +108,18 @@ pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]); 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); + pc.printf("Angle_Offset: %0.4f \r\n",kalman.ir.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); - } -} - // motion control thread ------------------------ void motion_thread(void const *argument) { motors.resetEncoders(); - motors.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2); - Thread::wait(1000); + //motors.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2); + //Thread::wait(1000); motors.stop(); - (*AI_Thread_Ptr).signal_set(0x01); - - + ai.thr_AI.signal_set(0x01); float currX, currY,currTheta; float speedL,speedR; @@ -222,7 +127,7 @@ float lastdiffSpeed = 0; while (1) { - if (flag_terminate) { + if (ai.flag_terminate) { terminate(); } @@ -235,11 +140,11 @@ // check if target reached ---------------------------------- - if ( ( abs(currX - targetX) < POSITION_TOR ) - &&( abs(currY - targetY) < POSITION_TOR ) + if ( ( abs(currX - ai.target.x) < POSITION_TOR ) + &&( abs(currY - ai.target.y) < POSITION_TOR ) ) { - diffDir = rectifyAng(currTheta - targetTheta); + diffDir = rectifyAng(currTheta - ai.target.theta); diffSpeed = diffDir / PI; if (abs(diffDir) > ANGLE_TOR) { @@ -252,7 +157,7 @@ } else { motors.stop(); Thread::wait(4000); - (*AI_Thread_Ptr).signal_set(0x01); + ai.thr_AI.signal_set(0x01); } } @@ -260,7 +165,9 @@ else { // calc direction to target - float targetDir = atan2(targetY - currY, targetX - currX); + float targetDir = atan2(ai.target.y - currY, ai.target.x - currX); + if (!ai.target.facing) + targetDir = PI - targetDir; diffDir = rectifyAng(currTheta - targetDir); @@ -292,88 +199,14 @@ speedR = (1-2*abs(diffSpeed))*MOVE_SPEED; speedL = MOVE_SPEED; } - - motors.setSpeed( int(speedL), int(speedR)); + if (ai.target.facing) + motors.setSpeed( int(speedL), int(speedR)); + else + 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