The Dream Team / Mbed 2 deprecated first_test

Pop lock n drop it

Dependencies:   m3pi_ng mbed

main.cpp@4:14b00a82d206, 2014-05-21 (annotated)

Committer:
vsal
Date:
Wed May 21 15:09:12 2014 +0000
Revision:
4:14b00a82d206
Parent:
3:6f773c2ba3a0
Child:
5:f3805a1f9047
Child:
7:d0c19729e7fd
final day 2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsal 0:80d9725a7342 1 #include "mbed.h"
vsal 0:80d9725a7342 2 #include "m3pi_ng.h"
vsal 3:6f773c2ba3a0 3 #include "DigitalIn.h"
vsal 0:80d9725a7342 4
vsal 0:80d9725a7342 5 DigitalOut myled(LED1);
vsal 0:80d9725a7342 6 m3pi huey;
vsal 3:6f773c2ba3a0 7 DigitalIn m3pi_IN[]={(p12),(p21)}; // IR-Sensor
vsal 0:80d9725a7342 8
vsal 1:ac7fc0b5f878 9
vsal 1:ac7fc0b5f878 10 void printBattery()
vsal 1:ac7fc0b5f878 11 {
vsal 0:80d9725a7342 12 //prints battery voltage on led screen
vsal 0:80d9725a7342 13 float bat = huey.battery();
vsal 1:ac7fc0b5f878 14 huey.printf("Bat volt");
vsal 1:ac7fc0b5f878 15 huey.locate(0,1);
vsal 1:ac7fc0b5f878 16 huey.printf("%f",bat);
vsal 1:ac7fc0b5f878 17 }
vsal 1:ac7fc0b5f878 18
vsal 4:14b00a82d206 19 float myLinePos(void)
vsal 4:14b00a82d206 20 {
vsal 4:14b00a82d206 21 int sensors[5];
vsal 4:14b00a82d206 22 huey.calibrated_sensor(sensors);
vsal 4:14b00a82d206 23 float leftSen, rightSen, midSen;
vsal 4:14b00a82d206 24 leftSen = sensors[1] / 1000;
vsal 4:14b00a82d206 25 rightSen = sensors[3] / 1000;
vsal 4:14b00a82d206 26 midSen = sensors[2] / 1000;
vsal 4:14b00a82d206 27 return rightSen - leftSen;
vsal 4:14b00a82d206 28 }
vsal 4:14b00a82d206 29
vsal 1:ac7fc0b5f878 30 void smoothFollow(float position, float speed)
vsal 1:ac7fc0b5f878 31 {
vsal 4:14b00a82d206 32 float u = speed;
vsal 1:ac7fc0b5f878 33 u = u * position;
vsal 1:ac7fc0b5f878 34 if(speed+u > 1)
vsal 1:ac7fc0b5f878 35 {
vsal 1:ac7fc0b5f878 36 huey.stop();
vsal 1:ac7fc0b5f878 37 huey.printf("Fast Er");
vsal 1:ac7fc0b5f878 38 }
vsal 1:ac7fc0b5f878 39 else
vsal 1:ac7fc0b5f878 40 {
vsal 2:e513ada0ecf1 41 huey.right_motor(speed-u);
vsal 2:e513ada0ecf1 42 huey.left_motor(speed+u);
vsal 1:ac7fc0b5f878 43 }
vsal 0:80d9725a7342 44
vsal 1:ac7fc0b5f878 45 }
vsal 1:ac7fc0b5f878 46
vsal 3:6f773c2ba3a0 47 void slowStop(float speed, float waitTime, int steps)
vsal 1:ac7fc0b5f878 48 {
vsal 3:6f773c2ba3a0 49 float i;
vsal 3:6f773c2ba3a0 50 for(i = speed; i > 0; i = i - (speed/steps))
vsal 1:ac7fc0b5f878 51 {
vsal 3:6f773c2ba3a0 52 smoothFollow(huey.line_position(),i);
vsal 3:6f773c2ba3a0 53 wait(waitTime);
vsal 1:ac7fc0b5f878 54 }
vsal 3:6f773c2ba3a0 55 huey.stop();
vsal 1:ac7fc0b5f878 56 }
vsal 1:ac7fc0b5f878 57
vsal 3:6f773c2ba3a0 58
vsal 3:6f773c2ba3a0 59 int pushToStart(void)
vsal 3:6f773c2ba3a0 60 {
vsal 3:6f773c2ba3a0 61 DigitalIn button(p21);
vsal 3:6f773c2ba3a0 62 int value = button.read();
vsal 3:6f773c2ba3a0 63 return value;
vsal 3:6f773c2ba3a0 64 }
vsal 3:6f773c2ba3a0 65
vsal 3:6f773c2ba3a0 66
vsal 3:6f773c2ba3a0 67 /////////////////////////////////////////////////////////////////
vsal 1:ac7fc0b5f878 68 int main() {
vsal 3:6f773c2ba3a0 69
vsal 3:6f773c2ba3a0 70 m3pi_IN[0].mode(PullUp);
vsal 3:6f773c2ba3a0 71 m3pi_IN[1].mode(PullUp);
vsal 3:6f773c2ba3a0 72
vsal 3:6f773c2ba3a0 73 int start;
vsal 3:6f773c2ba3a0 74 do
vsal 3:6f773c2ba3a0 75 {
vsal 3:6f773c2ba3a0 76 start = pushToStart();
vsal 3:6f773c2ba3a0 77 }while(start == 1);
vsal 3:6f773c2ba3a0 78
vsal 3:6f773c2ba3a0 79 wait(1);
vsal 1:ac7fc0b5f878 80 //calibrates sensors
vsal 1:ac7fc0b5f878 81 huey.sensor_auto_calibrate();
vsal 1:ac7fc0b5f878 82
vsal 3:6f773c2ba3a0 83 printBattery();
vsal 0:80d9725a7342 84
vsal 0:80d9725a7342 85
vsal 2:e513ada0ecf1 86 float speed = 0.25;
vsal 1:ac7fc0b5f878 87 float pos;
vsal 1:ac7fc0b5f878 88 int z=1;
vsal 1:ac7fc0b5f878 89 while(z==1)
vsal 1:ac7fc0b5f878 90 {
vsal 1:ac7fc0b5f878 91 //huey.right_motor(speed);
vsal 4:14b00a82d206 92 pos = myLinePos();//huey.line_position();
vsal 1:ac7fc0b5f878 93 smoothFollow(pos, speed);
vsal 3:6f773c2ba3a0 94 if(m3pi_IN[0]==0)
vsal 3:6f773c2ba3a0 95 {
vsal 3:6f773c2ba3a0 96 slowStop(speed, 0.05, 3);
vsal 3:6f773c2ba3a0 97 huey.stop();
vsal 3:6f773c2ba3a0 98 while(m3pi_IN[0]==0)
vsal 3:6f773c2ba3a0 99 {
vsal 3:6f773c2ba3a0 100 huey.printf("Stuck");
vsal 3:6f773c2ba3a0 101 }
vsal 3:6f773c2ba3a0 102 }
vsal 1:ac7fc0b5f878 103
vsal 1:ac7fc0b5f878 104 }
vsal 1:ac7fc0b5f878 105
vsal 1:ac7fc0b5f878 106
vsal 0:80d9725a7342 107
vsal 0:80d9725a7342 108
vsal 0:80d9725a7342 109
vsal 0:80d9725a7342 110
vsal 0:80d9725a7342 111 while(1)
vsal 0:80d9725a7342 112 {
vsal 0:80d9725a7342 113 myled = 1;
vsal 0:80d9725a7342 114 wait(0.2);
vsal 0:80d9725a7342 115 myled = 0;
vsal 0:80d9725a7342 116 wait(0.2);
vsal 0:80d9725a7342 117 }
vsal 0:80d9725a7342 118 }
vsal 1:ac7fc0b5f878 119