File content as of revision 3:36c4d1dbed7e:

#ifndef __ROBOT__C
#define __ROBOT__C

#include "Robot.h"

SRF05 sensor(p24,p25);


Serial pcr(USBTX, USBRX);

Robot::Robot() {
    led1 = new DigitalOut(LED1);
    led2 = new DigitalOut(LED2);
    led3 = new DigitalOut(LED3);

    compass=new CMPS03(p9,p10, CMPS03_DEFAULT_I2C_ADDRESS);
    wait(1);
    desiredHeading=compass->readBearing();

    pid = new PID(1.0, 0.00, 0.01, PID_RATE);
    pid->setInputLimits(-450,450);
    pid->setOutputLimits(-MAX_ROBOT_SPEED,MAX_ROBOT_SPEED);
    pid->setBias(0);
    pid->setMode(AUTO_MODE);
    pid->setSetPoint(0);

    range= new float[55];
    returnRange = new float[11];
//    sensor1=new SRF05 (p22, p23);
//    sensor=new SRF05 (p24, p25);

    isFullScan=0;
    readingNumber=0;

    servo=new Servo (p21);
    servo->calibrate(.0009, 180);
    servoPosition=5;
    servoDirection=1;
    *servo=(float)servoPosition/10;
    wait(1);



    ticker1.attach(this, &Robot::TickTick, PID_RATE*2);
//    ticker2.attach(this, &Robot::TackTack, SERVO_RATE);
    ticker3.attach(this, &Robot::FlashLEDs, 1);
}
Robot::~Robot() {
    delete compass;
    delete pid;
//    delete sensor;
    delete range;
    delete returnRange;
    delete servo;
}

int Robot::GetDesiredHeading() {
    return desiredHeading/10;
}
int Robot::GetActualHeading() {
    return actualHeading/10;
}
int Robot::SetDediredHeading(int deg) {
    deg%=360;
    if (deg<0)deg=deg+360;
    desiredHeading=deg*10;
    return deg;
}

void Robot::SetSpeed(int spd) {
    int tmp=0;
    desiredSpeed=spd%MAX_ROBOT_SPEED;
    if (desiredSpeed>0)tmp=desiredSpeed+50;
    if (desiredSpeed<0)tmp=desiredSpeed-50;
    base.SetPWM(tmp);
}

void Robot::SetSpeed(int lSpd, int rSpd) {
    lSpd%=MAX_ROBOT_SPEED;
    rSpd%=MAX_ROBOT_SPEED;
    if (lSpd>0)lSpd+=50;
    if (lSpd<0)lSpd-=50;
    if (rSpd>0)rSpd+=50;
    if (rSpd<0)rSpd-=50;
    base.SetPWM(lSpd,rSpd);
}
void Robot::SetLeftSpeed(int spd) {
    spd%=MAX_ROBOT_SPEED;
    if (spd>0)spd+=50;
    if (spd<0)spd-=50;
    base.SetLeftPWM(spd);
}

void Robot::SetRightSpeed(int spd) {
    spd%=MAX_ROBOT_SPEED;
    if (spd>0)spd+=50;
    if (spd<0)spd-=50;
//    pcr.printf("R:%i\t\t",spd);
    base.SetRightPWM(spd);
}

void Robot::Stop() {
    SetSpeed(0);
}

void Robot::ClearCount() {
    base.ClearCount();
}

float Robot::GetDistance() {
    return base.GetDistance();
}

int Robot::GetCount() {
    return base.GetCount();
}

void Robot::PivetRight(int deg){
    deg%=360;
    desiredHeading=(desiredHeading+deg*10)%3600;
    

}

void Robot::PivetLeft(int deg){
    deg%=360;
    desiredHeading=(desiredHeading-deg*10)%3600;
}

float Robot::GetRange(int position) {
    switch (position) {
        case 0:
            return range0/5;
        case 1:
            return range1/5;
        case 2:
            return range2/5;
        case 3:
            return range3/5;
        case 4:
            return range4/5;
        case 5:
            return range5/5;
        case 6:
            return range6/5;
        case 7:
            return range7/5;
        case 8:
            return range8/5;
        case 9:
            return range9/5;
        case 10:
            return range10/5;
        default:
            return -1.0;
    }
}


void Robot::TickTick() {
    actualHeading = (*compass).readBearing();
    deltaHeading=(desiredHeading-actualHeading);
    if (deltaHeading<-1800)deltaHeading+=3600;
    pid->setProcessValue(deltaHeading);
    float delta=pid->compute();//*((float)desiredSpeed/(MAX_ROBOT_SPEED));
//    pcr.printf("Desired Heading = %i,\t Actual Heading = %i,\t Delta Heading = %i\n\r",desiredHeading/10, actualHeading/10, deltaHeading/10);
//    pcr.printf("SPD= %i, Delta = %i, \tadjustment = %i\n\r",desiredSpeed, deltaHeading/10,(int)delta);
    if (desiredSpeed)
        SetSpeed(desiredSpeed-(int)delta, desiredSpeed+(int)delta);

}

void Robot::TackTack() {
    range[servoPosition*5+readingNumber] = sensor.read();
//   pcr.printf("range= %f\n\r",range[servoPosition*5+readingNumber]);
    range0=range[0+0]+range[0+1]+range[0+2]+range[0+3]+range[0+4];
    range1=range[5+0]+range[5+1]+range[5+2]+range[5+3]+range[5+4];
    range2=range[10+0]+range[10+1]+range[10+2]+range[10+3]+range[10+4];
    range3=range[15+0]+range[15+1]+range[15+2]+range[15+3]+range[15+4];
    range4=range[20+0]+range[20+1]+range[20+2]+range[20+3]+range[20+4];
    range5=range[25+0]+range[25+1]+range[25+2]+range[25+3]+range[25+4];
    range6=range[30+0]+range[30+1]+range[30+2]+range[30+3]+range[30+4];
    range7=range[35+0]+range[35+1]+range[35+2]+range[35+3]+range[35+4];
    range8=range[40+0]+range[40+1]+range[40+2]+range[40+3]+range[40+4];
    range9=range[45+0]+range[45+1]+range[45+2]+range[45+3]+range[45+4];
    range10=range[50+0]+range[50+1]+range[50+2]+range[50+3]+range[50+4];

    wait(SERVO_RATE/2);


    switch (isFullScan) {
        case 0:
            if (servoDirection==1&&servoPosition>=10) {
                readingNumber++;
                servoDirection=-1;
            }
            if (servoDirection==-1&&servoPosition<=0) {
                readingNumber++;
                servoDirection=1;
            }
            break;
        case 1:
        default:
            if (servoDirection==1&&servoPosition>=8) {
                readingNumber++;
                servoDirection=-1;
            }
            if (servoDirection==-1&&servoPosition<=3) {
                readingNumber++;
                servoDirection=1;
            }
    }
    servoPosition+=servoDirection;
//    pcr.printf("range= %f\n\r",servoPosition);
    *servo=(float)servoPosition/10;
    readingNumber%=5;

}

void Robot::FlashLEDs() {
    if (abs(deltaHeading)<100) *led2=1;
    else *led2=0;

    if (deltaHeading>50) *led1=1;
    else *led1=0;

    if (deltaHeading<-50) *led3=1;
    else *led3=0;
}


#endif