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@1:8638bdaf172b, 2016-12-06 (annotated)

Committer:
greiner218
Date:
Tue Dec 06 00:48:26 2016 +0000
Revision:
1:8638bdaf172b
Parent:
0:616fbf21a20e
Child:
2:64585b8d8404
Updated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
greiner218 0:616fbf21a20e 1 #include "mbed.h"
greiner218 0:616fbf21a20e 2 #include "rtos.h"
greiner218 0:616fbf21a20e 3 #include "Motor.h"
greiner218 1:8638bdaf172b 4 #include "Servo.h"
greiner218 1:8638bdaf172b 5 #include "LSM9DS1.h"
greiner218 0:616fbf21a20e 6
greiner218 1:8638bdaf172b 7 //MOTOR PINS
greiner218 1:8638bdaf172b 8 Motor rwheel(p22, p18, p17); // pwmA, fwd / Ain2, rev / Ain1
greiner218 1:8638bdaf172b 9 Motor lwheel(p23, p15, p16); // pwmB, fwd / Bin2, rev /
greiner218 1:8638bdaf172b 10 //BLUETOOTH PINS
greiner218 1:8638bdaf172b 11 Serial bt(p28,p27);
greiner218 1:8638bdaf172b 12 //LEFT SONAR
greiner218 1:8638bdaf172b 13 DigitalOut triggerL(p14);
greiner218 1:8638bdaf172b 14 DigitalIn echoL(p12);
greiner218 1:8638bdaf172b 15 //RIGHT SONAR
greiner218 1:8638bdaf172b 16 DigitalOut triggerR(p13);
greiner218 1:8638bdaf172b 17 DigitalIn echoR(p11);
greiner218 1:8638bdaf172b 18 //SERVO PINS
greiner218 1:8638bdaf172b 19 Servo angle(p21);
greiner218 1:8638bdaf172b 20 //IMU PINS
greiner218 1:8638bdaf172b 21 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
greiner218 1:8638bdaf172b 22
greiner218 1:8638bdaf172b 23 int correction = 0;
greiner218 1:8638bdaf172b 24 Timer sonar;
greiner218 1:8638bdaf172b 25
greiner218 1:8638bdaf172b 26 Serial pc(USBTX, USBRX);
greiner218 0:616fbf21a20e 27 Timer timestamp;
greiner218 1:8638bdaf172b 28 // Set up Hall-effect control
greiner218 1:8638bdaf172b 29 DigitalIn EncR(p19); // right motor
greiner218 1:8638bdaf172b 30 DigitalIn EncL(p20); // left motor
greiner218 1:8638bdaf172b 31 int oldEncR = 0; // Previous encoder values
greiner218 1:8638bdaf172b 32 int oldEncL = 0;
greiner218 1:8638bdaf172b 33 int ticksR = 0; // Encoder wheel state change counts
greiner218 1:8638bdaf172b 34 int ticksL = 0;
greiner218 1:8638bdaf172b 35 int E; // Error between the 2 wheel speeds
greiner218 1:8638bdaf172b 36 float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1)
greiner218 1:8638bdaf172b 37 float speedR = 0; //Same for right wheel
greiner218 1:8638bdaf172b 38 Ticker Sampler; //Interrupt Routine to sample encoder ticks.
greiner218 1:8638bdaf172b 39 int Instr = 0;
greiner218 0:616fbf21a20e 40
greiner218 1:8638bdaf172b 41
greiner218 1:8638bdaf172b 42 //Sample encoders and find error on right wheel. Assume Left wheel is always correct speed.
greiner218 1:8638bdaf172b 43 void sampleEncoder()
greiner218 1:8638bdaf172b 44 {
greiner218 1:8638bdaf172b 45 if((Instr == 1 || Instr == 2) && ticksL != 0) { //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped.
greiner218 1:8638bdaf172b 46
greiner218 1:8638bdaf172b 47 E = ticksL - ticksR; //Find error
greiner218 1:8638bdaf172b 48 speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
greiner218 1:8638bdaf172b 49 rwheel.speed(speedR);
greiner218 1:8638bdaf172b 50 }
greiner218 1:8638bdaf172b 51 ticksR = 0; //Restart the counters
greiner218 1:8638bdaf172b 52 ticksL = 0;
greiner218 1:8638bdaf172b 53 }
greiner218 0:616fbf21a20e 54
greiner218 0:616fbf21a20e 55 /* move(float, int)
greiner218 0:616fbf21a20e 56 Usage: Moves the robot forward in a straight line at a constant speed.
greiner218 0:616fbf21a20e 57 s is a float from -1 to 1 representing the speed to move.
greiner218 0:616fbf21a20e 58 A negative speed is used for backwards movement.
greiner218 0:616fbf21a20e 59 t is the time in milliseconds to move.
greiner218 0:616fbf21a20e 60 */
greiner218 0:616fbf21a20e 61 void move(float s, int t)
greiner218 0:616fbf21a20e 62 {
greiner218 0:616fbf21a20e 63 Timer timer;
greiner218 0:616fbf21a20e 64 //Start the timer
greiner218 0:616fbf21a20e 65 timer.start();
greiner218 0:616fbf21a20e 66 //Set the robot in motion at speed s
greiner218 0:616fbf21a20e 67 rwheel.speed(s);
greiner218 0:616fbf21a20e 68 lwheel.speed(s);
greiner218 0:616fbf21a20e 69 //Give other threads priority until the elapsed time
greiner218 0:616fbf21a20e 70 while(timer.read_ms() < t); //Thread::yield();
greiner218 0:616fbf21a20e 71 //Stop the robot
greiner218 0:616fbf21a20e 72 rwheel.speed(0);
greiner218 0:616fbf21a20e 73 lwheel.speed(0);
greiner218 0:616fbf21a20e 74 }
greiner218 0:616fbf21a20e 75
greiner218 0:616fbf21a20e 76 /* turn(float,int)
greiner218 0:616fbf21a20e 77 Usage: Turns the robot about its center at constant angular speed.
greiner218 0:616fbf21a20e 78 s is a float from -1 to 1 representing the speed to turn.
greiner218 0:616fbf21a20e 79 A positive s indicates a clockwise rotation.
greiner218 0:616fbf21a20e 80 A negative s indicates a counter-clockwise rotation.
greiner218 0:616fbf21a20e 81 */
greiner218 0:616fbf21a20e 82 void turn(float s, int t)
greiner218 0:616fbf21a20e 83 {
greiner218 0:616fbf21a20e 84 Timer timer;
greiner218 0:616fbf21a20e 85 //Start the timer
greiner218 0:616fbf21a20e 86 timer.start();
greiner218 0:616fbf21a20e 87 //Set the robot in motion turning at speed s
greiner218 0:616fbf21a20e 88 rwheel.speed(-1*s);
greiner218 0:616fbf21a20e 89 lwheel.speed(s);
greiner218 0:616fbf21a20e 90 //Give other threads priority until the elapsed time
greiner218 0:616fbf21a20e 91 while(timer.read_ms() < t);// Thread::yield();
greiner218 0:616fbf21a20e 92 //Stop the robot
greiner218 0:616fbf21a20e 93 rwheel.speed(0);
greiner218 0:616fbf21a20e 94 lwheel.speed(0);
greiner218 0:616fbf21a20e 95 }
greiner218 1:8638bdaf172b 96
greiner218 1:8638bdaf172b 97 int ping(int i)
greiner218 1:8638bdaf172b 98 {
greiner218 1:8638bdaf172b 99 int distance = 0;
greiner218 1:8638bdaf172b 100 switch(i)
greiner218 1:8638bdaf172b 101 {
greiner218 1:8638bdaf172b 102
greiner218 1:8638bdaf172b 103 case(1): //Ping Left Sonar
greiner218 1:8638bdaf172b 104 // trigger sonar to send a ping
greiner218 1:8638bdaf172b 105 triggerL = 1;
greiner218 1:8638bdaf172b 106 sonar.reset();
greiner218 1:8638bdaf172b 107 wait_us(10.0);
greiner218 1:8638bdaf172b 108 triggerL = 0;
greiner218 1:8638bdaf172b 109 //wait for echo high
greiner218 1:8638bdaf172b 110 while (echoL==0) {};
greiner218 1:8638bdaf172b 111 //echo high, so start timer
greiner218 1:8638bdaf172b 112 sonar.start();
greiner218 1:8638bdaf172b 113 //wait for echo low
greiner218 1:8638bdaf172b 114 while (echoL==1) {};
greiner218 1:8638bdaf172b 115 //stop timer and read value
greiner218 1:8638bdaf172b 116 sonar.stop();
greiner218 1:8638bdaf172b 117 //subtract software overhead timer delay and scale to cm
greiner218 1:8638bdaf172b 118 distance = (sonar.read_us()-correction)/58.0;
greiner218 1:8638bdaf172b 119 //wait so that any echo(s) return before sending another ping
greiner218 1:8638bdaf172b 120 wait(0.015);
greiner218 1:8638bdaf172b 121 break;
greiner218 1:8638bdaf172b 122 case(2): //Ping Right Sonar
greiner218 1:8638bdaf172b 123 // trigger sonar to send a ping
greiner218 1:8638bdaf172b 124 triggerR = 1;
greiner218 1:8638bdaf172b 125 sonar.reset();
greiner218 1:8638bdaf172b 126 wait_us(10.0);
greiner218 1:8638bdaf172b 127 triggerR = 0;
greiner218 1:8638bdaf172b 128 //wait for echo high
greiner218 1:8638bdaf172b 129 while (echoR==0) {};
greiner218 1:8638bdaf172b 130 //echo high, so start timer
greiner218 1:8638bdaf172b 131 sonar.start();
greiner218 1:8638bdaf172b 132 //wait for echo low
greiner218 1:8638bdaf172b 133 while (echoR==1) {};
greiner218 1:8638bdaf172b 134 //stop timer and read value
greiner218 1:8638bdaf172b 135 sonar.stop();
greiner218 1:8638bdaf172b 136 //subtract software overhead timer delay and scale to cm
greiner218 1:8638bdaf172b 137 distance = (sonar.read_us()-correction)/58.0;
greiner218 1:8638bdaf172b 138 //wait so that any echo(s) return before sending another ping
greiner218 1:8638bdaf172b 139 wait(0.015);
greiner218 1:8638bdaf172b 140 break;
greiner218 1:8638bdaf172b 141 default:
greiner218 1:8638bdaf172b 142 break;
greiner218 1:8638bdaf172b 143 }
greiner218 1:8638bdaf172b 144
greiner218 1:8638bdaf172b 145 return distance;
greiner218 1:8638bdaf172b 146 }
greiner218 1:8638bdaf172b 147
greiner218 0:616fbf21a20e 148 int main()
greiner218 0:616fbf21a20e 149 {
greiner218 0:616fbf21a20e 150 timestamp.start();
greiner218 1:8638bdaf172b 151 // Initialize hall-effect control
greiner218 1:8638bdaf172b 152 Sampler.attach(&sampleEncoder, 0.02); //Sampler uses sampleEncoder function every 20ms
greiner218 1:8638bdaf172b 153 EncL.mode(PullUp); // Use internal pullups
greiner218 1:8638bdaf172b 154 EncR.mode(PullUp);
greiner218 1:8638bdaf172b 155 while(1) {
greiner218 1:8638bdaf172b 156 Instr = 1;
greiner218 1:8638bdaf172b 157 switch (Instr) {
greiner218 1:8638bdaf172b 158 case 0://Stop
greiner218 1:8638bdaf172b 159 lwheel.speed(0);
greiner218 1:8638bdaf172b 160 rwheel.speed(0);
greiner218 1:8638bdaf172b 161 break;
greiner218 1:8638bdaf172b 162 case 1://Forward
greiner218 1:8638bdaf172b 163
greiner218 1:8638bdaf172b 164 speedL = 1;
greiner218 1:8638bdaf172b 165 //speedR = speedL;
greiner218 1:8638bdaf172b 166 lwheel.speed(speedL);
greiner218 1:8638bdaf172b 167 rwheel.speed(speedR);
greiner218 1:8638bdaf172b 168 break;
greiner218 1:8638bdaf172b 169 case 2://Reverse
greiner218 1:8638bdaf172b 170 speedL = -0.5;
greiner218 1:8638bdaf172b 171 speedR = speedL;
greiner218 1:8638bdaf172b 172 lwheel.speed(speedL);
greiner218 1:8638bdaf172b 173 rwheel.speed(speedR);
greiner218 1:8638bdaf172b 174 break;
greiner218 1:8638bdaf172b 175 case 3://Clockwise Turn
greiner218 1:8638bdaf172b 176 speedL = 0.3;
greiner218 1:8638bdaf172b 177 lwheel.speed(speedL);
greiner218 1:8638bdaf172b 178 rwheel.speed(-speedL);
greiner218 1:8638bdaf172b 179 break;
greiner218 1:8638bdaf172b 180 case 4://CCW Turn
greiner218 1:8638bdaf172b 181 speedL = -0.3;
greiner218 1:8638bdaf172b 182 lwheel.speed(-speedL);
greiner218 1:8638bdaf172b 183 rwheel.speed(speedL);
greiner218 1:8638bdaf172b 184 break;
greiner218 1:8638bdaf172b 185 }//End Switch
greiner218 1:8638bdaf172b 186
greiner218 1:8638bdaf172b 187 if(Instr == 1 || Instr == 2) { // Only increment tick values if moving forward or reverse
greiner218 1:8638bdaf172b 188 if(EncR != oldEncR) { // Increment ticks every time the state has switched.
greiner218 1:8638bdaf172b 189 ticksR++;
greiner218 1:8638bdaf172b 190 oldEncR = EncR;
greiner218 1:8638bdaf172b 191
greiner218 1:8638bdaf172b 192 }
greiner218 1:8638bdaf172b 193 if(EncL != oldEncR) {
greiner218 1:8638bdaf172b 194 ticksL++;
greiner218 1:8638bdaf172b 195 oldEncL = EncL;
greiner218 1:8638bdaf172b 196 }
greiner218 1:8638bdaf172b 197 pc.printf("tickR: %i \n\r",ticksR);
greiner218 1:8638bdaf172b 198 pc.printf("tickL: %i \n\r",ticksL);
greiner218 1:8638bdaf172b 199 }
greiner218 0:616fbf21a20e 200 }
greiner218 0:616fbf21a20e 201 }