Shuto Naruse
/
Eurobot_2012_Secondary
Eurobot_2012_Secondary
main.cpp
- Committer:
- narshu
- Date:
- 2012-04-20
- Revision:
- 0:fbfafa6bf5f9
File content as of revision 0:fbfafa6bf5f9:
#include "mbed.h" #include "rtos.h" #include "TSH.h" #include "Kalman.h" #include "globals.h" #include "motors.h" #include "math.h" #include "system.h" #include "geometryfuncs.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; // 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); //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! 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; 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(); //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!!!!!!!!!!!!!! while (1) { // do nothing Thread::wait(osWaitForever); } } void vMotorThread(void const *argument) { motors.resetEncoders(); while (1) { motors.setSpeed(20,20); Thread::wait(2000); motors.stop(); Thread::wait(5000); motors.setSpeed(-20,-20); Thread::wait(2000); motors.stop(); Thread::wait(5000); motors.setSpeed(-20,20); Thread::wait(2000); motors.stop(); Thread::wait(5000); motors.setSpeed(20,-20); Thread::wait(2000); motors.stop(); Thread::wait(5000); } } void vPrintState(void const *argument) { float state[3]; float SonarMeasures[3]; float IRMeasures[3]; while (1) { kalman.statelock.lock(); state[0] = kalman.X(0); state[1] = kalman.X(1); state[2] = kalman.X(2); SonarMeasures[0] = kalman.SonarMeasures[0]; SonarMeasures[1] = kalman.SonarMeasures[1]; SonarMeasures[2] = kalman.SonarMeasures[2]; IRMeasures[0] = kalman.IRMeasures[0]; IRMeasures[1] = kalman.IRMeasures[1]; 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("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); } } // 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); }