The Dream Team / Mbed 2 deprecated first_test

Pop lock n drop it

Dependencies:   m3pi_ng mbed

main.cpp@7:d0c19729e7fd, 2014-05-22 (annotated)

Committer:
vsal
Date:
Thu May 22 12:51:22 2014 +0000
Revision:
7:d0c19729e7fd
Parent:
4:14b00a82d206
Child:
8:09f4495696d2
Vinces code

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 7:d0c19729e7fd 23 //oat leftSen, rightSen, midSen;
vsal 7:d0c19729e7fd 24 //ftSen = sensors[1] / 1000;
vsal 7:d0c19729e7fd 25 //ghtSen = sensors[3] / 1000;
vsal 7:d0c19729e7fd 26 //dSen = sensors[2] / 1000;
vsal 7:d0c19729e7fd 27 float val;
vsal 7:d0c19729e7fd 28 val = (((1000*sensors[2] + 2000*sensors[3])/(sensors[1]+sensors[2]+sensors[3])));
vsal 7:d0c19729e7fd 29 return -1+(val/1000);
vsal 7:d0c19729e7fd 30
vsal 4:14b00a82d206 31 }
vsal 4:14b00a82d206 32
vsal 1:ac7fc0b5f878 33 void smoothFollow(float position, float speed)
vsal 1:ac7fc0b5f878 34 {
vsal 4:14b00a82d206 35 float u = speed;
vsal 1:ac7fc0b5f878 36 u = u * position;
vsal 1:ac7fc0b5f878 37 if(speed+u > 1)
vsal 1:ac7fc0b5f878 38 {
vsal 1:ac7fc0b5f878 39 huey.stop();
vsal 1:ac7fc0b5f878 40 huey.printf("Fast Er");
vsal 1:ac7fc0b5f878 41 }
vsal 1:ac7fc0b5f878 42 else
vsal 1:ac7fc0b5f878 43 {
vsal 2:e513ada0ecf1 44 huey.right_motor(speed-u);
vsal 2:e513ada0ecf1 45 huey.left_motor(speed+u);
vsal 1:ac7fc0b5f878 46 }
vsal 0:80d9725a7342 47
vsal 1:ac7fc0b5f878 48 }
vsal 1:ac7fc0b5f878 49
vsal 3:6f773c2ba3a0 50 void slowStop(float speed, float waitTime, int steps)
vsal 1:ac7fc0b5f878 51 {
vsal 3:6f773c2ba3a0 52 float i;
vsal 3:6f773c2ba3a0 53 for(i = speed; i > 0; i = i - (speed/steps))
vsal 1:ac7fc0b5f878 54 {
vsal 3:6f773c2ba3a0 55 smoothFollow(huey.line_position(),i);
vsal 3:6f773c2ba3a0 56 wait(waitTime);
vsal 1:ac7fc0b5f878 57 }
vsal 3:6f773c2ba3a0 58 huey.stop();
vsal 1:ac7fc0b5f878 59 }
vsal 1:ac7fc0b5f878 60
vsal 3:6f773c2ba3a0 61
vsal 3:6f773c2ba3a0 62 int pushToStart(void)
vsal 3:6f773c2ba3a0 63 {
vsal 3:6f773c2ba3a0 64 DigitalIn button(p21);
vsal 3:6f773c2ba3a0 65 int value = button.read();
vsal 3:6f773c2ba3a0 66 return value;
vsal 3:6f773c2ba3a0 67 }
vsal 3:6f773c2ba3a0 68
vsal 3:6f773c2ba3a0 69
vsal 3:6f773c2ba3a0 70 /////////////////////////////////////////////////////////////////
vsal 1:ac7fc0b5f878 71 int main() {
vsal 3:6f773c2ba3a0 72
vsal 7:d0c19729e7fd 73
vsal 3:6f773c2ba3a0 74 m3pi_IN[0].mode(PullUp);
vsal 3:6f773c2ba3a0 75 m3pi_IN[1].mode(PullUp);
vsal 3:6f773c2ba3a0 76
vsal 3:6f773c2ba3a0 77 int start;
vsal 3:6f773c2ba3a0 78 do
vsal 3:6f773c2ba3a0 79 {
vsal 3:6f773c2ba3a0 80 start = pushToStart();
vsal 3:6f773c2ba3a0 81 }while(start == 1);
vsal 3:6f773c2ba3a0 82
vsal 3:6f773c2ba3a0 83 wait(1);
vsal 1:ac7fc0b5f878 84 //calibrates sensors
vsal 1:ac7fc0b5f878 85 huey.sensor_auto_calibrate();
vsal 1:ac7fc0b5f878 86
vsal 3:6f773c2ba3a0 87 printBattery();
vsal 0:80d9725a7342 88
vsal 0:80d9725a7342 89
vsal 2:e513ada0ecf1 90 float speed = 0.25;
vsal 1:ac7fc0b5f878 91 float pos;
vsal 1:ac7fc0b5f878 92 int z=1;
vsal 1:ac7fc0b5f878 93 while(z==1)
vsal 1:ac7fc0b5f878 94 {
vsal 1:ac7fc0b5f878 95 //huey.right_motor(speed);
vsal 4:14b00a82d206 96 pos = myLinePos();//huey.line_position();
vsal 1:ac7fc0b5f878 97 smoothFollow(pos, speed);
vsal 3:6f773c2ba3a0 98 if(m3pi_IN[0]==0)
vsal 3:6f773c2ba3a0 99 {
vsal 3:6f773c2ba3a0 100 slowStop(speed, 0.05, 3);
vsal 3:6f773c2ba3a0 101 huey.stop();
vsal 3:6f773c2ba3a0 102 while(m3pi_IN[0]==0)
vsal 3:6f773c2ba3a0 103 {
vsal 3:6f773c2ba3a0 104 huey.printf("Stuck");
vsal 3:6f773c2ba3a0 105 }
vsal 3:6f773c2ba3a0 106 }
vsal 1:ac7fc0b5f878 107
vsal 1:ac7fc0b5f878 108 }
vsal 1:ac7fc0b5f878 109
vsal 1:ac7fc0b5f878 110
vsal 0:80d9725a7342 111
vsal 0:80d9725a7342 112
vsal 0:80d9725a7342 113
vsal 0:80d9725a7342 114
vsal 0:80d9725a7342 115 while(1)
vsal 0:80d9725a7342 116 {
vsal 0:80d9725a7342 117 myled = 1;
vsal 0:80d9725a7342 118 wait(0.2);
vsal 0:80d9725a7342 119 myled = 0;
vsal 0:80d9725a7342 120 wait(0.2);
vsal 0:80d9725a7342 121 }
vsal 0:80d9725a7342 122 }
vsal 1:ac7fc0b5f878 123