Ryan Greiner / Mbed 2 deprecated Robot

Programming for Shadow Robot control for smart phone mapping application.

Dependencies:   Motor LSM9DS1_Library_cal mbed Servo

main.cpp@2:64585b8d8404, 2016-12-07 (annotated)

Committer:
greiner218
Date:
Wed Dec 07 02:23:31 2016 +0000
Revision:
2:64585b8d8404
Parent:
1:8638bdaf172b
Child:
3:ce743dbbd4a5
Added bluetooth handler.; Changed Hall effect encoders to interrupts.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
greiner218 0:616fbf21a20e 1 #include "mbed.h"
greiner218 0:616fbf21a20e 2 #include "Motor.h"
greiner218 1:8638bdaf172b 3 #include "Servo.h"
greiner218 1:8638bdaf172b 4 #include "LSM9DS1.h"
greiner218 0:616fbf21a20e 5
greiner218 1:8638bdaf172b 6 //MOTOR PINS
greiner218 1:8638bdaf172b 7 Motor rwheel(p22, p18, p17); // pwmA, fwd / Ain2, rev / Ain1
greiner218 1:8638bdaf172b 8 Motor lwheel(p23, p15, p16); // pwmB, fwd / Bin2, rev /
greiner218 1:8638bdaf172b 9 //BLUETOOTH PINS
greiner218 1:8638bdaf172b 10 Serial bt(p28,p27);
greiner218 1:8638bdaf172b 11 //LEFT SONAR
greiner218 1:8638bdaf172b 12 DigitalOut triggerL(p14);
greiner218 1:8638bdaf172b 13 DigitalIn echoL(p12);
greiner218 1:8638bdaf172b 14 //RIGHT SONAR
greiner218 1:8638bdaf172b 15 DigitalOut triggerR(p13);
greiner218 1:8638bdaf172b 16 DigitalIn echoR(p11);
greiner218 1:8638bdaf172b 17 //SERVO PINS
greiner218 1:8638bdaf172b 18 Servo angle(p21);
greiner218 1:8638bdaf172b 19 //IMU PINS
greiner218 2:64585b8d8404 20 LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
greiner218 1:8638bdaf172b 21
greiner218 2:64585b8d8404 22 int correction = 0; //Used to adjust software delay for sonar measurement
greiner218 1:8638bdaf172b 23 Timer sonar;
greiner218 1:8638bdaf172b 24
greiner218 1:8638bdaf172b 25 Serial pc(USBTX, USBRX);
greiner218 0:616fbf21a20e 26 Timer timestamp;
greiner218 1:8638bdaf172b 27 // Set up Hall-effect control
greiner218 2:64585b8d8404 28 InterruptIn EncR(p25);
greiner218 2:64585b8d8404 29 InterruptIn EncL(p24);
greiner218 1:8638bdaf172b 30 int ticksR = 0; // Encoder wheel state change counts
greiner218 1:8638bdaf172b 31 int ticksL = 0;
greiner218 1:8638bdaf172b 32 float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1)
greiner218 1:8638bdaf172b 33 float speedR = 0; //Same for right wheel
greiner218 2:64585b8d8404 34 int dirL = 1; //1 for fwd, -1 for reverse
greiner218 2:64585b8d8404 35 int dirR = 1; //1 for fwd, -1 for reverse
greiner218 1:8638bdaf172b 36 Ticker Sampler; //Interrupt Routine to sample encoder ticks.
greiner218 2:64585b8d8404 37 int tracking = 0; //Flag used to indicate right wheel correction
greiner218 1:8638bdaf172b 38
greiner218 1:8638bdaf172b 39 //Sample encoders and find error on right wheel. Assume Left wheel is always correct speed.
greiner218 1:8638bdaf172b 40 void sampleEncoder()
greiner218 1:8638bdaf172b 41 {
greiner218 2:64585b8d8404 42 float E; // Error between the 2 wheel speeds
greiner218 2:64585b8d8404 43 if(tracking) { //Right wheel "tracks" left wheel if enabled.
greiner218 1:8638bdaf172b 44
greiner218 1:8638bdaf172b 45 E = ticksL - ticksR; //Find error
greiner218 2:64585b8d8404 46 //E = (ticksL+1)/(ticksR+1);
greiner218 2:64585b8d8404 47 if (dirL == 1)
greiner218 2:64585b8d8404 48 {
greiner218 2:64585b8d8404 49 speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
greiner218 2:64585b8d8404 50 //speedR = E*speedR;
greiner218 2:64585b8d8404 51 if (speedR < 0) speedR = 0;
greiner218 2:64585b8d8404 52 }
greiner218 2:64585b8d8404 53 else if (dirL == -1)
greiner218 2:64585b8d8404 54 {
greiner218 2:64585b8d8404 55 speedR = speedR - float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
greiner218 2:64585b8d8404 56 //speedR = E*speedR;
greiner218 2:64585b8d8404 57 if (speedR > 0) speedR = 0;
greiner218 2:64585b8d8404 58 }
greiner218 1:8638bdaf172b 59 rwheel.speed(speedR);
greiner218 1:8638bdaf172b 60 }
greiner218 2:64585b8d8404 61 //pc.printf("tickL: %i tickR: %i Error: %i\n\r ",ticksL, ticksR, ticksL - ticksR);
greiner218 1:8638bdaf172b 62 ticksR = 0; //Restart the counters
greiner218 1:8638bdaf172b 63 ticksL = 0;
greiner218 1:8638bdaf172b 64 }
greiner218 0:616fbf21a20e 65
greiner218 1:8638bdaf172b 66 int ping(int i)
greiner218 1:8638bdaf172b 67 {
greiner218 1:8638bdaf172b 68 int distance = 0;
greiner218 1:8638bdaf172b 69 switch(i)
greiner218 1:8638bdaf172b 70 {
greiner218 1:8638bdaf172b 71
greiner218 1:8638bdaf172b 72 case(1): //Ping Left Sonar
greiner218 1:8638bdaf172b 73 // trigger sonar to send a ping
greiner218 1:8638bdaf172b 74 triggerL = 1;
greiner218 1:8638bdaf172b 75 sonar.reset();
greiner218 1:8638bdaf172b 76 wait_us(10.0);
greiner218 1:8638bdaf172b 77 triggerL = 0;
greiner218 1:8638bdaf172b 78 //wait for echo high
greiner218 1:8638bdaf172b 79 while (echoL==0) {};
greiner218 1:8638bdaf172b 80 //echo high, so start timer
greiner218 1:8638bdaf172b 81 sonar.start();
greiner218 1:8638bdaf172b 82 //wait for echo low
greiner218 1:8638bdaf172b 83 while (echoL==1) {};
greiner218 1:8638bdaf172b 84 //stop timer and read value
greiner218 1:8638bdaf172b 85 sonar.stop();
greiner218 1:8638bdaf172b 86 //subtract software overhead timer delay and scale to cm
greiner218 1:8638bdaf172b 87 distance = (sonar.read_us()-correction)/58.0;
greiner218 1:8638bdaf172b 88 //wait so that any echo(s) return before sending another ping
greiner218 1:8638bdaf172b 89 wait(0.015);
greiner218 1:8638bdaf172b 90 break;
greiner218 1:8638bdaf172b 91 case(2): //Ping Right Sonar
greiner218 1:8638bdaf172b 92 // trigger sonar to send a ping
greiner218 1:8638bdaf172b 93 triggerR = 1;
greiner218 1:8638bdaf172b 94 sonar.reset();
greiner218 1:8638bdaf172b 95 wait_us(10.0);
greiner218 1:8638bdaf172b 96 triggerR = 0;
greiner218 1:8638bdaf172b 97 //wait for echo high
greiner218 1:8638bdaf172b 98 while (echoR==0) {};
greiner218 1:8638bdaf172b 99 //echo high, so start timer
greiner218 1:8638bdaf172b 100 sonar.start();
greiner218 1:8638bdaf172b 101 //wait for echo low
greiner218 1:8638bdaf172b 102 while (echoR==1) {};
greiner218 1:8638bdaf172b 103 //stop timer and read value
greiner218 1:8638bdaf172b 104 sonar.stop();
greiner218 1:8638bdaf172b 105 //subtract software overhead timer delay and scale to cm
greiner218 1:8638bdaf172b 106 distance = (sonar.read_us()-correction)/58.0;
greiner218 1:8638bdaf172b 107 //wait so that any echo(s) return before sending another ping
greiner218 1:8638bdaf172b 108 wait(0.015);
greiner218 1:8638bdaf172b 109 break;
greiner218 1:8638bdaf172b 110 default:
greiner218 1:8638bdaf172b 111 break;
greiner218 1:8638bdaf172b 112 }
greiner218 1:8638bdaf172b 113
greiner218 1:8638bdaf172b 114 return distance;
greiner218 1:8638bdaf172b 115 }
greiner218 1:8638bdaf172b 116
greiner218 2:64585b8d8404 117 void incTicksR()
greiner218 2:64585b8d8404 118 {
greiner218 2:64585b8d8404 119 ticksR++;
greiner218 2:64585b8d8404 120 }
greiner218 2:64585b8d8404 121
greiner218 2:64585b8d8404 122 void incTicksL()
greiner218 2:64585b8d8404 123 {
greiner218 2:64585b8d8404 124 ticksL++;
greiner218 2:64585b8d8404 125 }
greiner218 2:64585b8d8404 126
greiner218 0:616fbf21a20e 127 int main()
greiner218 0:616fbf21a20e 128 {
greiner218 2:64585b8d8404 129 char cmd;
greiner218 0:616fbf21a20e 130 timestamp.start();
greiner218 2:64585b8d8404 131 angle = 0.0f;
greiner218 1:8638bdaf172b 132 // Initialize hall-effect control
greiner218 2:64585b8d8404 133 Sampler.attach(&sampleEncoder, 0.2); //Sampler uses sampleEncoder function every 200ms
greiner218 1:8638bdaf172b 134 EncL.mode(PullUp); // Use internal pullups
greiner218 1:8638bdaf172b 135 EncR.mode(PullUp);
greiner218 2:64585b8d8404 136 EncR.rise(&incTicksR);
greiner218 2:64585b8d8404 137 EncL.rise(&incTicksL);
greiner218 2:64585b8d8404 138 while(1)
greiner218 2:64585b8d8404 139 {
greiner218 2:64585b8d8404 140 //Check if char is ready to be read and put into command buffer;
greiner218 2:64585b8d8404 141 if(bt.getc() =='!')
greiner218 2:64585b8d8404 142 {
greiner218 2:64585b8d8404 143 cmd = bt.getc();
greiner218 2:64585b8d8404 144 switch (cmd)
greiner218 2:64585b8d8404 145 {
greiner218 2:64585b8d8404 146 case 'B':
greiner218 2:64585b8d8404 147 pc.printf("Got Command B!\n\r");
greiner218 2:64585b8d8404 148 break;
greiner218 2:64585b8d8404 149
greiner218 2:64585b8d8404 150 case 'S': //Stop moving
greiner218 2:64585b8d8404 151 pc.printf("Got Command STOP!\n\r");
greiner218 2:64585b8d8404 152 tracking = 0; //Turn off wheel feedback updates
greiner218 2:64585b8d8404 153 speedL = 0;
greiner218 2:64585b8d8404 154 speedR = 0;
greiner218 2:64585b8d8404 155 lwheel.speed(0);
greiner218 2:64585b8d8404 156 rwheel.speed(0);
greiner218 2:64585b8d8404 157 break;
greiner218 2:64585b8d8404 158
greiner218 2:64585b8d8404 159 case 'F': //Forward
greiner218 2:64585b8d8404 160 pc.printf("Got Command FORWARD!\n\r");
greiner218 2:64585b8d8404 161 dirL = 1;
greiner218 2:64585b8d8404 162 dirR = 1;
greiner218 2:64585b8d8404 163 speedL = 0.9f;
greiner218 2:64585b8d8404 164 speedR = 0.9f;
greiner218 2:64585b8d8404 165 rwheel.speed(speedR);
greiner218 2:64585b8d8404 166 lwheel.speed(speedL);
greiner218 2:64585b8d8404 167 tracking = 1; //Turn on wheel feedback updates
greiner218 2:64585b8d8404 168 break;
greiner218 2:64585b8d8404 169
greiner218 2:64585b8d8404 170 case 'R': //Reverse
greiner218 2:64585b8d8404 171 pc.printf("Got Command REVERSE!\n\r");
greiner218 2:64585b8d8404 172 dirL = -1;
greiner218 2:64585b8d8404 173 dirR = -1;
greiner218 2:64585b8d8404 174 speedL = -0.9f;
greiner218 2:64585b8d8404 175 speedR = -0.9f;
greiner218 2:64585b8d8404 176 rwheel.speed(speedR);
greiner218 2:64585b8d8404 177 lwheel.speed(speedL);
greiner218 2:64585b8d8404 178 tracking = 1;
greiner218 2:64585b8d8404 179 break;
greiner218 2:64585b8d8404 180
greiner218 2:64585b8d8404 181 default:
greiner218 2:64585b8d8404 182 pc.printf("Unknown Command!\n\r");
greiner218 2:64585b8d8404 183 break;
greiner218 2:64585b8d8404 184
greiner218 1:8638bdaf172b 185 }
greiner218 1:8638bdaf172b 186 }
greiner218 0:616fbf21a20e 187 }
greiner218 0:616fbf21a20e 188 }