Shuto Naruse
/
Eurobot_2012_Secondary
Eurobot_2012_Secondary
Diff: main.cpp
- Revision:
- 0:fbfafa6bf5f9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Apr 20 21:32:24 2012 +0000 @@ -0,0 +1,379 @@ +#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); +} \ No newline at end of file