File content as of revision 0:6893dd9a88b3:

#include "mbed.h"
#include "Motor.h"
#include "movement.h"


void moveforward(Motor wheel,float speed){
    wheel.speed(speed);
}

void movebackward(Motor wheel,float speed){
    wheel.speed(-speed);
}

void move(int distance){
    rightspokecountrequired = abs(distance)/spokeconversionfactor;
    leftspokecountrequired = abs(distance)/spokeconversionfactor;
    if(distance > 0){
        moveforward(motor_right,rightmotorspeed);
        moveforward(motor_left,leftmotorspeed);
    }
    else if(distance < 0){
        movebackward(motor_right,rightmotorspeed);
        movebackward(motor_left,leftmotorspeed);
    }
}


void QTIsensor_init(){
    rightsensorcontrol = 0;
    leftsensorcontrol = 0;
}
 
void QTIsensor_charge(){
    rightsensorcontrol = 1;    
    leftsensorcontrol = 1;
}

void QTIsensor_discharge(){
    if(rightsensorval < qtithreshold) rightspokecount++;
    rightsensorcontrol = 0;
    if(leftsensorval < qtithreshold) leftspokecount++;
    leftsensorcontrol = 0;
}

void motors_stop(){
    motor_right.speed(0);
    motor_left.speed(0);
}

void calibratemotors(){
    int r,l;
    pc.printf("now moving forwards\n\r");
    move(2);
    while ((rightspokecount < rightspokecountrequired) || (leftspokecount < leftspokecountrequired)){
        pc.printf("right count = %d, left count = %d",rightspokecount,leftspokecount);
        wait(0.4);
    } 
    motors_stop();
    pc.printf("now moving backwards\n\r");
    wait(0.3);
    //rightspokecountrequired = 2/spokeconversionfactor;
    //leftspokecountrequired = 2/spokeconversionfactor;
    move(-2);
    pc.printf("right required = %d, left required = %d",rightspokecountrequired,leftspokecountrequired);
    while ((rightspokecount < rightspokecountrequired) && (leftspokecount < leftspokecountrequired)){
        pc.printf("right count = %d, left count = %d",rightspokecount,leftspokecount);
        wait(0.4);
    }
    r = rightspokecount;
    l = leftspokecount;
    while ((rightspokecount < rightspokecountrequired) || (leftspokecount < leftspokecountrequired)){
        pc.printf("right count = %d, left count = %d",rightspokecount,leftspokecount);
        wait(0.4);
    }
    motors_stop();
    wait(0.3);
    pc.printf("now re-calibrating\n\r");
    if(r != l){
        if((rightmotorspeed == 1)&&(leftmotorspeed == 1)){
            if(r>l){
                if((r - l) > 1){
                    rightmotorspeed = rightmotorspeed - 0.5;
                    calibratemotors();
                }
            }
            else if (r<l){
                if((l - r) > 1){
                    leftmotorspeed = leftmotorspeed - 0.5;
                    calibratemotors();
                }
            }
        }
        else if(rightmotorspeed == 1){
            if(r>l){
                if((r - l) > 1){
                    leftmotorspeed = leftmotorspeed + (1 - leftmotorspeed)*0.5;
                    calibratemotors();
                }
            }
            else if (r<l){
                if((l - r) > 1){
                    leftmotorspeed = leftmotorspeed - (1 - leftmotorspeed)*0.5;
                    calibratemotors();
                }
            }            
        }
        else if(leftmotorspeed == 1){
            if(r>l){
                if((r - l) > 1){
                    rightmotorspeed = rightmotorspeed - (1 - rightmotorspeed)*0.5;
                    calibratemotors();
                }
            }
            else if (r<l){
                if((l - r) > 1){
                    rightmotorspeed = rightmotorspeed + (1 - rightmotorspeed)*0.5;                
                    calibratemotors();
                }
            }            
        }
    }
}