Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Diff: main.cpp
- Revision:
- 0:f3bf6c7e2283
- Child:
- 1:bbabbd997d21
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Apr 20 20:39:35 2012 +0000 @@ -0,0 +1,374 @@ +#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 <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); + +int16_t face; + + +Motors motors; +Kalman kalman(motors); + +float targetX = 1000, targetY = 1000, targetTheta = 0; + +// bytes packing for IR turret serial comm +union IRValue_t { + float IR_floats[6]; + int IR_ints[6]; + unsigned char IR_chars[24]; +} IRValues; + +//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); +void vIRValueISR (void); +void motorSpeedThread(void const *argument); + +float getAngle (float x, float y); +void getIRValue(void const *argument); + + +//Thread thr_AI(ai_thread); +//Thread thr_motion(motion_thread); +//Thread thr_getIRValue(getIRValue); + +Mutex targetlock; +bool flag_terminate = false; + + +float temp = 0; + +//Main loop +int main() { +motors.stop(); + pc.baud(115200); + IRturret.baud(9600); + IRturret.format(8,Serial::Odd,1); + //Thread thread(motorSpeedThread, NULL, osPriorityAboveNormal); //PID controller task. run at 50ms + //IRturret.attach(&vIRValueISR); + //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256); + Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024); + + 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 motorSpeedThread(void const *argument) { + while (1) { + motors.speedRegulatorTask(); + Thread::wait(10); + } +} + + +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); + } +} + + + +void vPrintState(void const *argument) { + float state[3]; + float sonardist[3]; + //float x,y; + //float d = beaconpos[2].y - beaconpos[1].y; + //float i = beaconpos[0].y; + //float j = beaconpos[0].x; + + while (1) { + kalman.statelock.lock(); + state[0] = kalman.X(0); + state[1] = kalman.X(1); + state[2] = kalman.X(2); + sonardist[0] = kalman.SonarMeasures[0]*1000.0f; + sonardist[1] = kalman.SonarMeasures[1]*1000.0f; + sonardist[2] = kalman.SonarMeasures[2]*1000.0f; + kalman.statelock.unlock(); + + // trilateration algorithm + //y = ((sonardist[1]*sonardist[1]) - (sonardist[2]*sonardist[2]) + d*d) / (2*d); + //x = ((sonardist[1]*sonardist[1]) - (sonardist[0]*sonardist[0]) + i*i + j*j) / (2*j) - (i/j)*y; + + + //kalman.statelock.lock(); + pc.printf("\r\n"); + //printf("Position X: %0.1f Y: %0.1f \r\n", x,y); + pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]); + //targetlock.lock(); + //pc.printf("target : %0.4f %0.4f %0.4f \r\n", targetX/1000, targetY/1000,targetTheta); + //targetlock.unlock(); + printf("Sonar Dist: %0.4f, %0.4f, %0.4f \r\n", sonardist[0],sonardist[1],sonardist[2]); + + //printf("Distance is %0.1f, %0.1f \r\n", motors.encoderToDistance(motors.getEncoder1()), motors.encoderToDistance(motors.getEncoder2())); + + //pc.printf("IR ang : %f \n",temp * 180/3.14); + //kalman.statelock.unlock(); + Thread::wait(200); + } +} + + +// 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; + 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(20,20); + Thread::wait(2000); + motors.stop(); + //thr_AI.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(); + + + + // get IR turret angle -------------------------------------- + /* float angEst = getAngle(currX, currY); // check this + if (angEst != 1000){ + // here the value should be a valid estimate + // so update kalman state + + } + */ + //!!! add code here to update kalman angle state !!! + + // check if target reached ---------------------------------- + if ( ( abs(currX - targetX) < POSITION_TOR ) + &&( abs(currY - targetY) < POSITION_TOR ) + ) { + + diffDir = rectifyAng(currTheta - targetTheta); + diffSpeed = diffDir / PI; + + //pc.printf("%f \n",diffDir); + 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); + //thr_AI.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 getIRValue(void const *argument) { + Thread::signal_wait(0x01); + if (IRturret.readable() >= 24) { + for (int i=0; i < 24; i++) { + IRValues.IR_chars[i] = IRturret.getc(); + } + } +} + +void vIRValueISR (void) { + //thr_getIRValue.signal_set(0x01); +} + + +float getAngle (float x, float y) { + /* function that returns current robot orientation + input: x, y coords in mm + output: orientation in rad + note: the angle readings from IR is read in here + must be called faster than IR turret frequency (few Hz - currently 3Hz) + */ + + // variables + float ang0 = 0, ang1 = 0, ang2 = 0; + int sc0 = 0, sc1 = 0, sc2 = 0; + + + + // read serial port + if (IRturret.readable()) { + IRturret.scanf("%f;%f;%f;%d;%d;%d;",&ang0,&ang1,&ang2,&sc0,&sc1,sc2); + } + + // if none sampled + if (!sc0 && !sc1 && ! sc2) { + return 1000; + } + + // + else { + float est0 = 0, est1 = 0, est2 = 0; + + // calc + if ((sc0 > RELI_BOUND_LOW)||(sc0 < RELI_BOUND_HIGH)) { + est0 = atan2(1000 - y, 3000-x) - ang0; + } + if ((sc1 > RELI_BOUND_LOW)||(sc1 < RELI_BOUND_HIGH)) { + est1 = atan2( -y, -x) - ang1; + } + if ((sc2 > RELI_BOUND_LOW)||(sc2 < RELI_BOUND_HIGH)) { + est2 = atan2(2000 - y, -x) - ang2; + } + + // weighted avg + return rectifyAng( ((est0 * RELI_BOUND_HIGH-sc0) + (est1 * RELI_BOUND_HIGH-sc1) + (est2 * RELI_BOUND_HIGH-sc2)) / (3*RELI_BOUND_HIGH - sc0 - sc1 - sc2) ); + } + +} + +