The Dream Team / Mbed 2 deprecated first_test

Pop lock n drop it

Dependencies:   m3pi_ng mbed

main.cpp@1:ac7fc0b5f878, 2014-05-21 (annotated)

Committer:
vsal
Date:
Wed May 21 11:31:57 2014 +0000
Revision:
1:ac7fc0b5f878
Parent:
0:80d9725a7342
Child:
2:e513ada0ecf1
First try actually

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 0:80d9725a7342 3
vsal 0:80d9725a7342 4 DigitalOut myled(LED1);
vsal 0:80d9725a7342 5 m3pi huey;
vsal 0:80d9725a7342 6
vsal 1:ac7fc0b5f878 7 //stops without hitting
vsal 1:ac7fc0b5f878 8 void slowStop(float speed)
vsal 1:ac7fc0b5f878 9 {
vsal 1:ac7fc0b5f878 10 float i;
vsal 1:ac7fc0b5f878 11 for(i = speed; i > 0; i = i - 0.05)
vsal 1:ac7fc0b5f878 12 {
vsal 1:ac7fc0b5f878 13 huey.forward(i);
vsal 1:ac7fc0b5f878 14 wait(0.075);
vsal 1:ac7fc0b5f878 15 }
vsal 1:ac7fc0b5f878 16 huey.stop();
vsal 1:ac7fc0b5f878 17 }
vsal 1:ac7fc0b5f878 18
vsal 1:ac7fc0b5f878 19 void printBattery()
vsal 1:ac7fc0b5f878 20 {
vsal 0:80d9725a7342 21 //prints battery voltage on led screen
vsal 0:80d9725a7342 22 float bat = huey.battery();
vsal 1:ac7fc0b5f878 23 huey.printf("Bat volt");
vsal 1:ac7fc0b5f878 24 huey.locate(0,1);
vsal 1:ac7fc0b5f878 25 huey.printf("%f",bat);
vsal 1:ac7fc0b5f878 26 }
vsal 1:ac7fc0b5f878 27
vsal 1:ac7fc0b5f878 28 void smoothFollow(float position, float speed)
vsal 1:ac7fc0b5f878 29 {
vsal 1:ac7fc0b5f878 30 float u = speed/4;
vsal 1:ac7fc0b5f878 31 u = u * position;
vsal 1:ac7fc0b5f878 32 if(speed+u > 1)
vsal 1:ac7fc0b5f878 33 {
vsal 1:ac7fc0b5f878 34 huey.stop();
vsal 1:ac7fc0b5f878 35 huey.printf("Fast Er");
vsal 1:ac7fc0b5f878 36 }
vsal 1:ac7fc0b5f878 37 else
vsal 1:ac7fc0b5f878 38 {
vsal 1:ac7fc0b5f878 39 huey.right_motor(speed+u);
vsal 1:ac7fc0b5f878 40 huey.left_motor(speed-u);
vsal 1:ac7fc0b5f878 41 }
vsal 0:80d9725a7342 42
vsal 1:ac7fc0b5f878 43 }
vsal 1:ac7fc0b5f878 44
vsal 1:ac7fc0b5f878 45 void follow(float position, float speed)
vsal 1:ac7fc0b5f878 46 {
vsal 1:ac7fc0b5f878 47 float tol = 0.05;
vsal 1:ac7fc0b5f878 48 if(position > tol)
vsal 1:ac7fc0b5f878 49 {
vsal 1:ac7fc0b5f878 50 huey.right(speed);
vsal 1:ac7fc0b5f878 51 }
vsal 1:ac7fc0b5f878 52 else if(position < (-tol))
vsal 1:ac7fc0b5f878 53 {
vsal 1:ac7fc0b5f878 54 huey.left(speed);
vsal 1:ac7fc0b5f878 55 }
vsal 1:ac7fc0b5f878 56 else
vsal 1:ac7fc0b5f878 57 {
vsal 1:ac7fc0b5f878 58 huey.forward(speed);
vsal 1:ac7fc0b5f878 59 }
vsal 1:ac7fc0b5f878 60
vsal 1:ac7fc0b5f878 61 }
vsal 1:ac7fc0b5f878 62
vsal 1:ac7fc0b5f878 63 int main() {
vsal 1:ac7fc0b5f878 64 //calibrates sensors
vsal 1:ac7fc0b5f878 65 huey.sensor_auto_calibrate();
vsal 1:ac7fc0b5f878 66
vsal 1:ac7fc0b5f878 67 //printBattery();
vsal 0:80d9725a7342 68
vsal 0:80d9725a7342 69
vsal 1:ac7fc0b5f878 70 //huey.printf("%f", huey.line_position());
vsal 0:80d9725a7342 71
vsal 1:ac7fc0b5f878 72 float speed = 0.1;
vsal 1:ac7fc0b5f878 73 float pos;
vsal 1:ac7fc0b5f878 74 int z=1;
vsal 1:ac7fc0b5f878 75 while(z==1)
vsal 1:ac7fc0b5f878 76 {
vsal 1:ac7fc0b5f878 77 //huey.right_motor(speed);
vsal 1:ac7fc0b5f878 78 pos = huey.line_position();
vsal 1:ac7fc0b5f878 79 huey.printf("%f",pos );
vsal 1:ac7fc0b5f878 80 smoothFollow(pos, speed);
vsal 1:ac7fc0b5f878 81
vsal 1:ac7fc0b5f878 82 }
vsal 1:ac7fc0b5f878 83
vsal 1:ac7fc0b5f878 84
vsal 0:80d9725a7342 85
vsal 0:80d9725a7342 86
vsal 0:80d9725a7342 87
vsal 0:80d9725a7342 88
vsal 0:80d9725a7342 89 while(1)
vsal 0:80d9725a7342 90 {
vsal 0:80d9725a7342 91 myled = 1;
vsal 0:80d9725a7342 92 wait(0.2);
vsal 0:80d9725a7342 93 myled = 0;
vsal 0:80d9725a7342 94 wait(0.2);
vsal 0:80d9725a7342 95 }
vsal 0:80d9725a7342 96 }
vsal 1:ac7fc0b5f878 97