Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
main.cpp
- Committer:
- narshu
- Date:
- 2012-04-27
- Revision:
- 7:f9c59a3e4155
- Parent:
- 4:7b7334441da9
- Child:
- 9:377560539b74
File content as of revision 7:f9c59a3e4155:
#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 "ai.h" #include "ui.h" //#include <iostream> //Interface declaration Serial pc(USBTX, USBRX); // tx, rx Motors motors; UI ui; Kalman kalman(motors,ui,p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11); 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! void vMotorThread(void const *argument); void vPrintState(void const *argument); void motion_thread(void const *argument); //bool flag_terminate = false; float temp = 0; //Main loop int main() { pc.baud(115200); //Init kalman kalman.KalmanInit(); Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256); //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024); //Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024); //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",kalman.ir.angleOffset*180/PI); Thread::wait(100); } } // 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.thr_AI.signal_set(0x01); float currX, currY,currTheta; float speedL,speedR; float diffDir,diffSpeed; float lastdiffSpeed = 0; while (1) { if (ai.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 - ai.target.x) < POSITION_TOR ) &&( abs(currY - ai.target.y) < POSITION_TOR ) ) { diffDir = rectifyAng(currTheta - ai.target.theta); 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.thr_AI.signal_set(0x01); } } // adjust motion to reach target ---------------------------- else { // calc direction to target float targetDir = atan2(ai.target.y - currY, ai.target.x - currX); if (!ai.target.facing) targetDir = PI - targetDir; 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; } if (ai.target.facing) motors.setSpeed( int(speedL), int(speedR)); else motors.setSpeed( -int(speedL), -int(speedR)); } } // wait Thread::wait(MOTION_UPDATE_PERIOD); } }