Programming for Shadow Robot control for smart phone mapping application.
Dependencies: Motor LSM9DS1_Library_cal mbed Servo
main.cpp
- Committer:
- greiner218
- Date:
- 2016-12-09
- Revision:
- 5:48c8fb81288e
- Parent:
- 4:bc55afdd43de
File content as of revision 5:48c8fb81288e:
#include "mbed.h" #include "Motor.h" #include "Servo.h" #include "LSM9DS1.h" #include <math.h> #define TIMEOUT_VAL_MS 15 #define PI 3.14159 #define DECLINATION -4.94 #define DIST_FACTOR 0.107475 //cm/tick Conversion between a tick from Hall effect sensor and linear distance. //Approximately 190 ticks for a full wheel rotation. //Approximately 6.5cm wheel diameter //pi*diameter (cm) / 190(ticks) volatile float heading; float prevHeading[3]; void getHeading(float ax, float ay, float az, float mx, float my, float mz); float PI_cont(float err); //MOTOR PINS Motor rwheel(p22, p18, p17); // pwmA, fwd / Ain2, rev / Ain1 Motor lwheel(p23, p15, p16); // pwmB, fwd / Bin2, rev / //BLUETOOTH PINS Serial bt(p28,p27); //LEFT SONAR DigitalOut triggerL(p14); DigitalIn echoL(p12); DigitalOut echo1RiseWait(LED1); DigitalOut echo1FallWait(LED2); //RIGHT SONAR DigitalOut triggerR(p13); DigitalIn echoR(p11); DigitalOut echo2RiseWait(LED3); DigitalOut echo2FallWait(LED4); int correction = 0; //Used to adjust software delay for sonar measurement Timer sonar; Timer timeout; //SERVO PINS Servo angle(p21); //IMU PINS LSM9DS1 imu(p9, p10, 0xD6, 0x3C); Ticker nav; float posX = 0; float posY = 0; int tickDistL = 0; int tickDistR = 0; Serial pc(USBTX, USBRX); Timer timestamp; // Set up Hall-effect control InterruptIn EncR(p25); InterruptIn EncL(p24); int ticksR = 0; // Encoder wheel state change counts int ticksL = 0; float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1) float speedR = 0; //Same for right wheel int dirL = 1; //1 for fwd, -1 for reverse int dirR = 1; //1 for fwd, -1 for reverse Ticker Sampler; //Interrupt Routine to sample encoder ticks. int tracking = 0; //Flag used to indicate right wheel correction int sweeping = 0; //PI controller //global variables float kp=12.0f; float ki=4.0f; float kd=1.0f; float P=0; float I=0; float D=0; float dt=0.1f; float out=0.0f; float max=0.5f; float min=-0.5f; float prerr=0.0f; //SLAM variables float angle_list[64]; float dist_list[64]; int slam_ind = 0; float slam_dist = 0.0f; //Sample encoders and find error on right wheel. Assume Left wheel is always correct speed. void sampleEncoder() { float E; // Error between the 2 wheel speeds if(tracking) { //Right wheel "tracks" left wheel if enabled. E = ticksL - ticksR; //Find error //E = (ticksL+1)/(ticksR+1); if (dirL == 1) { speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error //speedR = E*speedR; if (speedR < 0) speedR = 0; } else if (dirL == -1) { speedR = speedR - float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error //speedR = E*speedR; if (speedR > 0) speedR = 0; } rwheel.speed(speedR); } //pc.printf("tickL: %i tickR: %i Error: %i\n\r ",ticksL, ticksR, ticksL - ticksR); ticksR = 0; //Restart the counters ticksL = 0; } float angleAdjust(float a) { while(a<0 || a>360) { if (a<0) a+=360; if (a>360) a-=360; } return a; } void getHeading(float ax, float ay, float az, float mx, float my, float mz) { float roll = atan2(ay, az); float pitch = atan2(-ax, sqrt(ay * ay + az * az)); //Shift previous headings prevHeading[2] = prevHeading[1]; prevHeading[1] = prevHeading[0]; prevHeading[0] = heading; // touchy trig stuff to use arctan to get compass heading (scale is 0..360) mx = -mx; if (my == 0.0) heading = (mx < 0.0) ? 180.0 : 0.0; else heading = atan2(mx, my)*360.0/(2.0*PI); //pc.printf("heading atan=%f \n\r",heading); heading -= DECLINATION; //correct for geo location heading += 90; //if(heading>180.0) heading = heading - 360.0; //else if(heading<-180.0) heading = 360.0 + heading; //else if(heading<0.0) heading = 360.0 + heading; heading = angleAdjust(heading); heading = (heading + prevHeading[0] + prevHeading[1] + prevHeading[2])/4.0; } float PI_cont(float err) { float Pout=kp*err; P=Pout; //pc.printf("P is: %f \n\r",P); float Iout=Iout+err*dt; I=ki*Iout; //pc.printf("I is: %f \n\r",I); float Dout = (err - prerr)/dt; D=kd*Dout; out=P+I+D; prerr=err; //pc.printf("out is: %f \n\r",out); // basically pwm out if (out>max)out=max; else if (out<min)out=min; return out; } void turn(float angle) { float s =0.05;//speed by which the robot should turn while(!imu.magAvailable(X_AXIS)); imu.readMag(); while(!imu.accelAvailable()); imu.readAccel(); while(!imu.gyroAvailable()); imu.readGyro(); getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx), imu.calcMag(imu.my), imu.calcMag(imu.mz)); float PV = heading;//the original heading float SP = heading - angle;//the new required heading if(SP>=360){//reseting after 360 degrees to 0 SP = SP-360; } pc.printf("PV: %f\n",PV); pc.printf("SV: %f\n",SP); wait(2); //pc.printf("required: %f\n",requiredHeading); float error = angle; pc.printf("error: %f\n",error); int reached = 0; while(reached ==0){ while(!imu.magAvailable(ALL_AXIS)); imu.readMag(); getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx), imu.calcMag(imu.my), imu.calcMag(imu.mz)); PV = heading;//calculate the current heading error=(PV-SP)/360;//find error between current heading and required heading float correction=PI_cont(error);//correction to motor speed if (error> 1.5/360 || error<-1.5/360) { //an error limit of 1.5 degrees allowed.. checking if the robot needs to turn more rwheel.speed((s+correction)/2); lwheel.speed((s-correction)/2); pc.printf("error in 1st if: %f\n\r",error); pc.printf("error in 1st if: %f\n\r",error); } else if (error>=-1.5/360 && error<=1.5/360) { //within within satisfying angular range rwheel.speed(0); lwheel.speed(0);//stop moving and exit the loop pc.printf("error in 2nd if: %f\n\r",error); break; } wait(0.01); } rwheel.speed(0); lwheel.speed(0); } void sendCmd(char cmd, float arg) { bt.printf("!%c%.2f",cmd, arg); //unsigned char c[sizeof arg]; //memcpy(c, &arg, sizeof arg); //bt.putc('!'); //bt.putc(cmd); //bt.putc(c[0]); //bt.putc(c[1]); //bt.putc(c[2]); //bt.putc(c[3]); } int ping(int i) { float distance = 0; switch(i) { case(1): //Ping Left Sonar // trigger sonar to send a ping //pc.printf("Pinging sonar 1...\n\r"); triggerL = 1; sonar.reset(); //wait_us(10.0); //wait for echo high echo1RiseWait = 1; timeout.reset(); timeout.start(); while (echoL==0){ if (timeout.read_ms() > TIMEOUT_VAL_MS) return 0; }; echo1RiseWait = 0; //echo high, so start timer sonar.start(); triggerL = 0; //wait for echo low echo1FallWait = 1; timeout.reset(); timeout.start(); while (echoL==1) { if (timeout.read_ms() > TIMEOUT_VAL_MS*5) return 0; }; echo1FallWait = 0; //stop timer and read value sonar.stop(); //subtract software overhead timer delay and scale to cm distance = (sonar.read_us()-correction)/58.0; //pc.printf("Got distance %f cm back.\n\r", distance); //wait so that any echo(s) return before sending another ping wait(0.015); break; case(2): //Ping Right Sonar // trigger sonar to send a ping //pc.printf("Pinging sonar 2...\n\r"); triggerR = 1; sonar.reset(); //wait_us(10.0); //wait for echo high echo2RiseWait = 1; timeout.reset(); timeout.start(); while (echoR==0) { if (timeout.read_ms() > TIMEOUT_VAL_MS) return 0; }; echo2RiseWait = 0; //echo high, so start timer sonar.start(); triggerR = 0; //wait for echo low echo2FallWait = 1; timeout.reset(); timeout.start(); while (echoR==1) { if (timeout.read_ms() > TIMEOUT_VAL_MS*5) return 0; }; echo2FallWait = 0; //stop timer and read value sonar.stop(); //subtract software overhead timer delay and scale to cm distance = (sonar.read_us()-correction)/58.0; //pc.printf("Got distance %f cm back.\n\r", distance); //wait so that any echo(s) return before sending another ping wait(0.015); break; default: break; } return distance; } void sweep() { __disable_irq(); float AL = 0.0; float AR = 0.0; int dL = 0; int dR = 0; sweeping = 1; if (angle <=0.5) //Set to min angle and execute forward sweep. { angle = 0; wait(.2); while( angle >= 0 && angle < 1) { dL = ping(1); dR = ping(2); AL = angleAdjust(heading + (angle-0.5)*55 - 45); AR = angleAdjust(heading + (angle-0.5)*55 + 45); if (dL > 20 && dL < 200) { sendCmd('A',AL); sendCmd('D',dL); dist_list[slam_ind] = dL; angle_list[slam_ind] = AL-heading; if (slam_ind < 63) slam_ind++; pc.printf("(%.1f,%.1f)\n\r",AL-heading,dL); } if (dR > 20 && dR < 200) { sendCmd('A',AR); sendCmd('D',dR); dist_list[slam_ind] = dL; angle_list[slam_ind] = AR-heading; if (slam_ind < 63) slam_ind++; pc.printf("(%.1f,%.1f)\n\r",AR-heading,dR); } angle = angle + 0.05; } angle = 1; } else if (angle > 0.5) //Set to max angle and execute backward sweep. { angle = 1; wait(.2); while( angle > 0 && angle <= 1) { dL = ping(1); dR = ping(2); AL = angleAdjust(heading + (angle-0.5)*55 - 45); AR = angleAdjust(heading + (angle-0.5)*55 + 45); if (dL > 20 && dL < 200) { sendCmd('A',AL); sendCmd('D',dL); dist_list[slam_ind] = dL; angle_list[slam_ind] = AL-heading; if (slam_ind < 63) slam_ind++; pc.printf("(%.1f,%.1f)\n\r",AL-heading,dL); } if (dR > 20 && dR < 200) { sendCmd('A',AR); sendCmd('D',dR); dist_list[slam_ind] = dL; angle_list[slam_ind] = AR-heading; if (slam_ind < 63) slam_ind++; pc.printf("(%.1f,%.1f)\n\r",AR-heading,dR); } angle = angle - 0.05; } angle = 0; } sweeping = 0; __enable_irq(); } void updatePos() { if(!sweeping) { float deltaX; float deltaY; float dist; while(!imu.magAvailable(X_AXIS)); imu.readMag(); while(!imu.accelAvailable()); imu.readAccel(); while(!imu.gyroAvailable()); imu.readGyro(); getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx), imu.calcMag(imu.my), imu.calcMag(imu.mz)); dist = (tickDistR + tickDistL)/2.0f*DIST_FACTOR; dist = (dirL + dirR)*dist/2.0; //Average the ticks and convert to linear distance. slam_dist += dist; // pc.printf("Magnetic Heading: %f degress ",heading); //pc.printf("L ticks: %d R ticks: %d ",tickDistL, tickDistR); tickDistR = 0;//Reset the ticks. tickDistL = 0; deltaX = dist*(float)sin((double)heading*PI/180.0f); //Do trig. deltaY = dist*(float)cos((double)heading*PI/180.0f); posX = posX + deltaX; posY = posY + deltaY; //pc.printf("deltaX: %f deltaY: %f ",deltaX,deltaY); //pc.printf("posX: %f posY:%f \n\r",posX,posY); sendCmd('X',posX); sendCmd('Y',posY); sendCmd('H',heading); } } void incTicksR() { ticksR++; tickDistR++; } void incTicksL() { ticksL++; tickDistL++; } void moveFwd() { dirL = 1; dirR = 1; speedL = 0.9f; speedR = 0.9f; rwheel.speed(speedR); lwheel.speed(speedL); tracking = 1; //Turn on wheel feedback updates tickDistL = 0; //Reset the distance ticks. tickDistR = 0; } void moveBwd() { dirL = -1; dirR = -1; speedL = -0.9f; speedR = -0.9f; rwheel.speed(speedR); lwheel.speed(speedL); tracking = 1; tickDistL = 0; //Reset the distance ticks. tickDistR = 0; } void moveStp() { tracking = 0; //Turn off wheel feedback updates speedL = 0; speedR = 0; lwheel.speed(0); rwheel.speed(0); } void turnL() { tracking = 0; dirL = -1; dirR = 1; speedL = -0.9; speedR = .9; lwheel.speed(speedL); rwheel.speed(speedR); } void turnR() { tracking = 0; dirL = 1; dirR = -1; speedL = 0.9; speedR = -0.9; lwheel.speed(speedL); rwheel.speed(speedR); } int slam_sweep() { //Reset the SLAM variables; memset(angle_list,0,sizeof(angle_list)); memset(dist_list,0,sizeof(dist_list)); slam_ind = 0; //And sweep sweep(); //Iterate to determine if detection in "danger zone" for (int i = 0; i<64; i++) { if ((angle_list[i]) < 80 && (angle_list[i]) > -80) { if (dist_list[i] < 50 && dist_list[i] > 1) { return 1; } } } return 0; } void slam() { //Initialize arrays int detect = 0; wait(1); while(1) { while(!detect)//while an object is not within 30cm of front 60 degrees of robot... { //move forward 20cm and scan moveFwd(); //while(slam_dist < 20)pc.printf("SlamDist: %f\n\r",slam_dist); wait(.8); moveStp(); slam_dist = 0; detect = slam_sweep(); } //Else, turn left slightly. detect = 0; turnL(); wait(.8); moveStp(); //and scan detect = slam_sweep(); } } int main() { char cmd; timestamp.start(); angle = 0.0f; //Calibrate the IMU if (!imu.begin()) { pc.printf("Failed to communicate with LSM9DS1.\n"); } imu.calibrate(); turnR(); imu.calibrateMag(0); moveStp(); // Initialize hall-effect control Sampler.attach(&sampleEncoder, 0.2); //Sampler uses sampleEncoder function every 200ms EncL.mode(PullUp); // Use internal pullups EncR.mode(PullUp); EncR.rise(&incTicksR); EncL.rise(&incTicksL); memset(prevHeading, 0, sizeof(prevHeading)); nav.attach(&updatePos,1);//Update X,Y,H every second while(1) { //Check if char is ready to be read and put into command buffer; if(bt.getc() =='!') { cmd = bt.getc(); switch (cmd) { case 'B': //pc.printf("A button was pressed!\n\r"); cmd = bt.getc(); switch(cmd) { case '5': //Up cmd = bt.getc(); switch(cmd) { case '1': //Pressed moveFwd(); break; case '0': //Released moveStp(); sweep(); break; } break; case '6': //Down cmd = bt.getc(); switch(cmd) { case '1': //Pressed moveBwd(); break; case '0': //Released moveStp(); sweep(); break; } break; case '7': //Left cmd = bt.getc(); switch(cmd) { case '1': //Pressed turnL(); break; case '0': //Released moveStp(); sweep(); break; } break; case '8': //Right cmd = bt.getc(); switch(cmd) { case '1': //Pressed turnR(); break; case '0': //Released moveStp(); sweep(); break; } break; case '9': //Right cmd = bt.getc(); switch(cmd) { case '1': //Pressed slam(); break; case '0': //Released break; } break; } break; case 'S': //Stop moving pc.printf("Got Command STOP!\n\r"); tracking = 0; //Turn off wheel feedback updates speedL = 0; speedR = 0; lwheel.speed(0); rwheel.speed(0); break; case 'F': //Forward pc.printf("Got Command FORWARD!\n\r"); dirL = 1; dirR = 1; speedL = 0.9f; speedR = 0.9f; rwheel.speed(speedR); lwheel.speed(speedL); tracking = 1; //Turn on wheel feedback updates tickDistL = 0; //Reset the distance ticks. tickDistR = 0; break; case 'R': //Reverse pc.printf("Got Command REVERSE!\n\r"); dirL = -1; dirR = -1; speedL = -0.9f; speedR = -0.9f; rwheel.speed(speedR); lwheel.speed(speedL); tracking = 1; tickDistL = 0; //Reset the distance ticks. tickDistR = 0; break; case 'P': //Sweep pc.printf("Got Command SWEEP!\n\r"); sweep(); break; default: pc.printf("Unknown Command!\n\r"); break; } } } }