File content as of revision 20:f0ca65974329:

#include "robot.h"

/*
 * This function will be called at approximately <SPEED> when the control mode is LINE_FOLLOW_MODE
 */
 
 ///////////////////////////////////////////add line following code
void line_follow_loop(){
    led4 = 1;
    
    //varaibles to
    float prev_position = 0;
    float derivative, proportional, integral = 0;
    int sensors[5];
    float position = 0;
    float power;    //speed increase or decrease
    float speed = MAX;


    //get sensors
    pi.sensor_reading(sensors);
    
    //line position
    position = pi.line_position();
    proportional = position;
    
    //derivative
    derivative = position - prev_position;
    
    //integral
    integral += proportional;
    
    //last position
    prev_position = position;
    
    power = (proportional * k_p) + (integral * k_i) + (derivative * k_d);
    
    //new speeds
    rightspeed = speed - power;
    leftspeed = speed + power;
    
   if(rightspeed < MIN)
        rightspeed = MIN;
    else if(rightspeed > MAX)
        rightspeed = MAX;
    
    if(leftspeed < MIN)
        leftspeed = MIN;
    else if(leftspeed > MAX)
        leftspeed = MAX;
    
    //pi.left_motor(leftspeed);
   // pi.right_motor(rightspeed);
    
    if(sensors[0] < 1200 && sensors[1] < 1200 && sensors[2] < 1200 && sensors[3] < 1200 && sensors[4] < 1200)
    {
        pi.stop();
        //dead end
        mode = MANUAL_MODE;
    }
    else if(sensors[0] > 1800 || sensors[4] > 1800)
    {
        //intersection
        mode = MANUAL_MODE;
    }

    // set mode to MANUAL_MODE when the end is detected
    // mode = MANUAL_MODE
    
    // wait_ms(10);

    led4 = 0;
}


// INPUT COMMANDS
// l:<float>    set left wheel speed (only effective in MANUAL_MODE)
// r:<float>    set right wheel speed (only effective in MANUAL_MODE)
// c:<int>      change mode
// g:<p|i|d>:<float>
//              change gains

// OUTPUT MESSAGES
// p:<float>    line position
// s:<int>,<int>,<int>,<int>,<int>
//              light sensor values
// m:<int>      mode

int parse_command(const char* cmd)
{
    if(cmd[1]==':'){
        // left motor
        if(cmd[0]=='l'){
            if(mode==MANUAL_MODE){
                leftspeed = atof(&cmd[2]);
            }
        // right motor
        }else if(cmd[0]=='r'){
            if(mode==MANUAL_MODE){
                rightspeed = atof(&cmd[2]);
            }
        // mode
        }else if(cmd[0]=='c'){
            mode = atoi(&cmd[2]);
            xbee.printf("mode set to %d\n", mode);
        // gains
        }else if(cmd[0]=='g'){
            if(cmd[2]=='p'){
                k_p = atof(&cmd[4]);
            }else if(cmd[2]=='i'){
                k_i = atof(&cmd[4]);
            }else if(cmd[2]=='d'){
                k_d = atof(&cmd[4]);
            }
            xbee.printf("gains p:%f, i:%f, d:%f\n", k_p, k_i, k_d);
        }else{
            xbee.printf("%s\n",cmd); 
        }
    }else{
        xbee.printf("%s\n",cmd);
    }
    return 0;
}

void Rx_interrupt()
{
    // assume recursive interrupt is not possible
    led2 = 1;
    char read;
    
    while(xbee.readable()){
        read = xbee.getc();
        if(read=='\n'){
            received[r_index]='\0'; // put a null character at the end
            parse_command(received);
            r_index=0;
        } else {
            if(r_index >= 80){
                r_index=0;
            }
            received[r_index]=read;
            r_index++;
        }
    }
    
    led2=0;
    
    return;
}

void communicate()
{
    led1 = 1;
    pi.sensor_reading(sensors);
    int* s = sensors; // just to make the next line more compact
    xbee.printf("s:%i,%i,%i,%i,%i\n", s[0], s[1], s[2], s[3], s[4]);
    xbee.printf("p:%f\n", pi.line_position());
    xbee.printf("m:%d\n", mode);
    led1 = 0;
}

void control()
{
    if(mode==LINE_FOLLOW_MODE){
        line_follow_loop();
    }
}

int main() {
    
    xbee.attach(&Rx_interrupt, Serial::RxIrq);
    xbeeReset = 0;
    wait(2);
    xbeeReset = 1;
    pi.sensor_auto_calibrate();
    leftspeed = 0.0;
    rightspeed = 0.0;
    
    r_index = 0;
    received[0] = '\0';
    mode = MANUAL_MODE;
    
    communication.attach(&communicate, 0.1);
    controls.attach(&control, 0.05);

    while(1){
        led3 = mode;
        //wait_ms(25);
        
        /*
        if(mode==LINE_FOLLOW_MODE){
            line_follow_loop();
        }
        */

        // reversing these is more intuitive
        // pi.left_motor(leftspeed);
        // pi.right_motor(rightspeed);
        __disable_irq();
        pi.left_motor(rightspeed);
        pi.right_motor(leftspeed);
        __enable_irq();

        /*
        led1 = 0;
        wait_ms(25);
        
        /*
        pi.sensor_reading(sensors);
        int* s = sensors;
        __disable_irq();
        xbee.printf("s:%i,%i,%i,%i,%i\n", s[0], s[1], s[2], s[3], s[4]);
        __enable_irq();
        __disable_irq();
        xbee.printf("p:%f\n", pi.line_position());
        __enable_irq();
        __disable_irq();
        xbee.printf("m:%d\n", mode);
        __enable_irq();
        */
    }

    return 0;
}