der Roboter / S1

Library for using the xbee-compatible Bluetooth Bee Module on an m3pi.

Dependencies:   SensoryTest

Fork of btbee by Nikolas Goldin

main.cpp@18:eddba761c425, 2014-05-21 (annotated)

Committer:
mmpeter
Date:
Wed May 21 15:00:52 2014 +0000
Revision:
18:eddba761c425
Parent:
17:a6cb4c42308c 
Turnarounds

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mmpeter 8:fd8a30a6923d 1 #include "mbed.h"
mmpeter 8:fd8a30a6923d 2 #include "m3pi_ng.h"
mmpeter 17:a6cb4c42308c 3 #include "cmath"
mmpeter 17:a6cb4c42308c 4 #include "iostream"
mmpeter 17:a6cb4c42308c 5
mmpeter 17:a6cb4c42308c 6 using namespace std;
mmpeter 8:fd8a30a6923d 7
mmpeter 8:fd8a30a6923d 8 m3pi thinggy;
mmpeter 8:fd8a30a6923d 9
mmpeter 8:fd8a30a6923d 10
mmpeter 8:fd8a30a6923d 11 int main() {
mmpeter 8:fd8a30a6923d 12
mmpeter 16:2b437630e29e 13 float speed = 0.25;
mmpeter 11:45172e28ecb2 14 float correction;
mmpeter 16:2b437630e29e 15 float k = -0.3;
mmpeter 16:2b437630e29e 16 int sensor[5];
mmpeter 17:a6cb4c42308c 17 int returned;
mmpeter 9:6ae4359b73df 18 thinggy.locate(0,1);
mmpeter 17:a6cb4c42308c 19 thinggy.printf("Villan");
mmpeter 17:a6cb4c42308c 20 thinggy.locate(0,0);
mmpeter 17:a6cb4c42308c 21 thinggy.printf("Pimpin");
mmpeter 8:fd8a30a6923d 22
mmpeter 10:7ef481d28dcd 23 wait(1.0);
mmpeter 17:a6cb4c42308c 24
mmpeter 8:fd8a30a6923d 25 thinggy.sensor_auto_calibrate();
mmpeter 17:a6cb4c42308c 26
mmpeter 17:a6cb4c42308c 27 thinggy.calibrated_sensor(sensor);
mmpeter 18:eddba761c425 28 returned = (sensor[1] + sensor[2] + sensor[3])/3;
mmpeter 18:eddba761c425 29
mmpeter 17:a6cb4c42308c 30
mmpeter 17:a6cb4c42308c 31
mmpeter 8:fd8a30a6923d 32 while(1) {
mmpeter 17:a6cb4c42308c 33 // change "if" to while
mmpeter 18:eddba761c425 34 while(returned < 220){
mmpeter 17:a6cb4c42308c 35 while(sensor[0] < sensor[4]){
mmpeter 18:eddba761c425 36 thinggy.left_motor(.1);
mmpeter 18:eddba761c425 37 thinggy.right_motor(-.1);
mmpeter 17:a6cb4c42308c 38 thinggy.calibrated_sensor(sensor);
mmpeter 17:a6cb4c42308c 39 }
mmpeter 17:a6cb4c42308c 40 while(sensor[4] > sensor[0]){
mmpeter 18:eddba761c425 41 thinggy.left_motor(-.1);
mmpeter 18:eddba761c425 42 thinggy.right_motor(.1);
mmpeter 17:a6cb4c42308c 43 thinggy.calibrated_sensor(sensor);
mmpeter 17:a6cb4c42308c 44 }
mmpeter 17:a6cb4c42308c 45 thinggy.calibrated_sensor(sensor);
mmpeter 18:eddba761c425 46 returned = (sensor[1] + sensor[2] + sensor[3])/3;
mmpeter 17:a6cb4c42308c 47 }
mmpeter 11:45172e28ecb2 48
mmpeter 18:eddba761c425 49 // Curves and straightaways
mmpeter 18:eddba761c425 50 while(returned > 220){
mmpeter 17:a6cb4c42308c 51
mmpeter 11:45172e28ecb2 52 float position = thinggy.line_position();
mmpeter 11:45172e28ecb2 53 correction = k*(position);
mmpeter 17:a6cb4c42308c 54
mmpeter 17:a6cb4c42308c 55 // -1.0 is far left, 1.0 is far right, 0.0 in the middle
mmpeter 11:45172e28ecb2 56
mmpeter 11:45172e28ecb2 57 //speed limiting for right motor
mmpeter 11:45172e28ecb2 58 if(speed + correction > 1){
mmpeter 15:cc5b8a858c6e 59 thinggy.right_motor(0.6);
mmpeter 17:a6cb4c42308c 60 }
mmpeter 11:45172e28ecb2 61 else{
mmpeter 11:45172e28ecb2 62 thinggy.right_motor(speed+correction);
mmpeter 11:45172e28ecb2 63 }
mmpeter 11:45172e28ecb2 64
mmpeter 11:45172e28ecb2 65 //speed limiting for left motor
mmpeter 11:45172e28ecb2 66 if(speed - correction > 1){
mmpeter 15:cc5b8a858c6e 67 thinggy.left_motor(0.6);
mmpeter 15:cc5b8a858c6e 68 }
mmpeter 11:45172e28ecb2 69 else{
mmpeter 10:7ef481d28dcd 70 thinggy.left_motor(speed-correction);
mmpeter 11:45172e28ecb2 71 }
mmpeter 17:a6cb4c42308c 72 thinggy.calibrated_sensor(sensor);
mmpeter 18:eddba761c425 73 returned = (sensor[1] + sensor[2] + sensor[3])/3;
mmpeter 17:a6cb4c42308c 74 }
mmpeter 17:a6cb4c42308c 75 thinggy.calibrated_sensor(sensor);
mmpeter 18:eddba761c425 76 returned = (sensor[1] + sensor[2] + sensor[3])/3;
mmpeter 17:a6cb4c42308c 77 }
mmpeter 16:2b437630e29e 78 }
mmpeter 16:2b437630e29e 79
mmpeter 17:a6cb4c42308c 80
mmpeter 13:4016c362b0d5 81
mmpeter 17:a6cb4c42308c 82