der Roboter / Mbed 2 deprecated BlueToothStuff_v2

Dependencies:   m3pi_ng mbed

Fork of BlueToothStuff_v2 by Stephen Tyson

main.cpp

Committer:
mmpeter
Date:
2014-05-22
Revision:
0:386c250325ce
Child:
1:6402638c6f6d

File content as of revision 0:386c250325ce:

#include "mbed.h"
#include "m3pi_ng.h"
#include "cmath"
#include "iostream"
 
using namespace std;
 
m3pi thinggy;
 
 
int main() {
    
    float speed = 0.25;
    float correction;
    float k = -0.3;
    int sensor[5];
    int returned;   
    thinggy.locate(0,1);
    thinggy.printf("Villan");
    thinggy.locate(0,0);
    thinggy.printf("Pimpin");
    
    wait(1.0);
 
    thinggy.sensor_auto_calibrate();
 
    thinggy.calibrated_sensor(sensor);
    returned = (sensor[1] + sensor[2] + sensor[3])/3;
 
    
    
    while(1) {
        // change "if" to while
        while(returned < 220){
                while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
                    thinggy.left_motor(.2);
                    thinggy.right_motor(-.2);
                    thinggy.calibrated_sensor(sensor);
                }
                while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
                    thinggy.left_motor(-.2);
                    thinggy.right_motor(.2);
                    thinggy.calibrated_sensor(sensor);
                }
                thinggy.calibrated_sensor(sensor);
                returned = (sensor[1] + sensor[2] + sensor[3])/3;
        }
        
        // Curves and straightaways    
        while(returned > 220){
 
        float position = thinggy.line_position();
        correction = k*(position);
       
         // -1.0 is far left, 1.0 is far right, 0.0 in the middle
        
        //speed limiting for right motor
        if(speed + correction > 1){
            thinggy.right_motor(0.6);
            thinggy.left_motor(speed-correction);
        } 
        else if(speed - correction > 1){
            thinggy.right_motor(speed+correction);
            thinggy.left_motor(0.6);
        }
        else{
            thinggy.left_motor(speed-correction);
            thinggy.right_motor(speed+correction);
        }
         thinggy.calibrated_sensor(sensor);
         returned = (sensor[1] + sensor[2] + sensor[3])/3;   
        }
    thinggy.calibrated_sensor(sensor);
    returned = (sensor[1] + sensor[2] + sensor[3])/3;
    }
}