Dependencies:   mbed

main.cpp

Committer:
microsat
Date:
2010-11-11
Revision:
0:301acbabd57a
Child:
1:e046cce6ceb8

File content as of revision 0:301acbabd57a:

#include "mbed.h"
#include "m3pi.h"

BusOut leds(LED1,LED2,LED3,LED4);
m3pi m3pi(p23,p9,p10);

#define MAX 1.0
#define MIN 0
#define P_TERM 1
#define I_TERM 0
#define D_TERM 20

int main() {

    m3pi.locate(0,1);
    m3pi.printf("Line Flw");

    wait(2.0);

    float right;
	float left;
	float position_of_line = 0.0;
    float prev_pos_of_line = 0.0;
	float derivative,proportional;
	float integral = 0;
	float power;
    m3pi.sensor_auto_calibrate();
    float speed = MAX;
	
    while (1) {

        // Get the position of the line.
        position_of_line = m3pi.line_position();
		proportional = position_of_line;
		// Compute the derivative
        derivative = position_of_line - prev_pos_of_line;
		// Compute the integral
		integral += proportional;

        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
		// Remember the last position.
        prev_pos_of_line = position_of_line;        
        right = speed+power;
        left  = speed-power;
        
        if (right < MIN)
            right = MIN;
        else if (right > MAX)
            right = MAX;
            
        if (left < MIN)
            left = MIN;
        else if (left > MAX)
            left = MAX;
            
       // set speed 
        m3pi.left_motor(left);
        m3pi.right_motor(right);


    }
}