The Dream Team / Mbed 2 deprecated first_test

Pop lock n drop it

Dependencies:   m3pi_ng mbed

main.cpp@3:6f773c2ba3a0, 2014-05-21 (annotated)

Committer:
vsal
Date:
Wed May 21 13:54:40 2014 +0000
Revision:
3:6f773c2ba3a0
Parent:
2:e513ada0ecf1
Child:
4:14b00a82d206
hhhh

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