File content as of revision 1:b4d202b471ae:

#include "Robot.h"
#include "mbed.h"

// This library was designed simplify operation of Stinger robot only with MBED and two H-Bridges.
// Is is still in development.
// Class Robot integrated QEI, PID and Motor classes to work together through use of Ticker function to 
// regularly poll QEI objects, adjust PID and Motor objects.
// Right now all pins have been preassigned, later empty constructor will be added with individual pin assignments.
//    Pinouts are:
//    rightMotor = new Motor(p23, p6, p5); // pwm, fwd, rev
//    leftMotor = new Motor(p22, p8, p7); // pwm, fwd, rev
//    leftQei = new QEI(p29,  p30, NC, 624, QEI::X2_ENCODING);  //chanA, chanB, index, ppr
//    rightQei = new QEI(p27, p28, NC, 624, QEI::X2_ENCODING);  //chanB, chanA, index, ppr
//    leftPid = new PID(0.4312, 0.1, 0.0, RATE);  //Kc, Ti, Td
//    rightPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td

Robot::Robot() {
    rightMotor = new Motor(p23, p6, p5); // pwm, fwd, rev
    leftMotor = new Motor(p22, p8, p7); // pwm, fwd, rev
    leftQei = new QEI(p29,  p30, NC, 624, QEI::X2_ENCODING);  //chanA, chanB, index, ppr
    rightQei = new QEI(p27, p28, NC, 624, QEI::X2_ENCODING);  //chanB, chanA, index, ppr
    leftPid = new PID(0.4312, 0.1, 0.0, RATE);  //Kc, Ti, Td
    rightPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td
    leftMotor->period(0.00005);  //Set motor PWM periods to 20KHz.
    rightMotor->period(0.00005);
    leftPid->setInputLimits(0, MAX_SPEED);
    leftPid->setOutputLimits(0.3, 1);
    leftPid->setMode(AUTO_MODE);
    rightPid->setInputLimits(0, MAX_SPEED);
    rightPid->setOutputLimits(0.3, 1);
    rightPid->setMode(AUTO_MODE);
    leftPulsesGoTo=rightPulsesGoTo=0;
    ticker.attach(this, &Robot::Call, RATE); // the address of the function to be attached (call) and the interval (RATE)
}

Robot::~Robot() {
    delete rightMotor;
    delete leftMotor;
    delete leftQei;
    delete rightQei;
    delete leftMotor;
    delete rightMotor;
    delete leftPid;
    delete rightPid;
    ticker.detach();
}

void Robot::MoveStraightPulses(float s, int pulses) {
    leftQei->reset();
    rightQei->reset();
    leftPulses=rightPulses=0;
    if (s>=0.0)leftDirection=rightDirection=1.0;
    else leftDirection=rightDirection=-1.0;
    leftSpeed=abs(s*MAX_SPEED);
    rightSpeed=abs(s*MAX_SPEED);
    leftPid->reset();
    rightPid->reset();
    leftPid->setSetPoint(leftSpeed);
    rightPid->setSetPoint(rightSpeed);
    leftPulsesGoTo=rightPulsesGoTo=pulses*2;
    if (s==0)Stop();
}

void Robot::MoveStraightInches(float speed, float in){
    MoveStraightPulses(speed, (int)(in*85.6144));
}

void Robot::MoveStraightRotations(float speed, float rotations){
    MoveStraightPulses(speed, (int)(rotations*624.00));
}

void Robot::Stop() {
    this->StopLeft();
    this->StopRight();
}

void Robot::StopLeft() {
    leftMotor->brake();
    leftPulsesGoTo=0;
    leftSpeed=0.0;
    leftPulses=0;
    leftQei->reset();
    
    leftPid->reset();
    leftPid->setSetPoint(0);
}

void Robot::StopRight() {
    rightMotor->brake();
    rightPulsesGoTo=0;
    rightSpeed=0.0;
    rightPulses=0;
    rightQei->reset();
    rightPid->reset();
    rightPid->setSetPoint(0);
    

}
void Robot::RotateLeftWheel(float speed, float deg) {
    leftPid->reset();
    leftSpeed=abs(speed*MAX_SPEED);
    if (speed>=0.0)leftDirection=1.0;
    else leftDirection=-1.0;
    leftPulsesGoTo=deg*624/360;
    leftPid->setSetPoint(leftSpeed);
}

void Robot::RotateRightWheel(float speed, float deg) {
    rightPid->reset();
    rightSpeed=abs(speed*MAX_SPEED);
    if (speed>=0.0)rightDirection=1.0;
    else leftDirection=-1.0;
    rightPulsesGoTo=deg*624/360;
    rightPid->setSetPoint(rightSpeed);
}

void Robot::PivetLeft(float deg) {
    if (deg<0.0) {
        PivetRight(-deg);
        return;
    }
    leftPid->reset();
    rightPid->reset();
    leftSpeed=abs(0.5*MAX_SPEED);
    rightSpeed=abs(0.5*MAX_SPEED);
    leftDirection=-1.0;
    rightDirection=1.0;
    leftPulsesGoTo=abs(deg*DPP);
    rightPulsesGoTo=abs(deg*DPP);
    leftPid->setSetPoint(leftSpeed);
    rightPid->setSetPoint(rightSpeed);
}

void Robot::PivetRight(float deg) {
    if (deg<0) {
        PivetLeft(-deg);
        return;
    }
    leftPid->reset();
    rightPid->reset();
    leftSpeed=abs(0.5*MAX_SPEED);
    rightSpeed=abs(0.5*MAX_SPEED);
    leftDirection=1.0;
    rightDirection=-1.0;
    leftPulsesGoTo=abs(deg*DPP);
    rightPulsesGoTo=abs(deg*DPP);
    leftPid->setSetPoint(leftSpeed);
    rightPid->setSetPoint(rightSpeed);
}

int  Robot::IsBusy() {
    if ((rightSpeed==0.0)&&(leftSpeed==0.0))return 0;
    return 1;
}

void Robot::Call() {
    float t;
    if ((rightPulsesGoTo!=0)&&(rightSpeed>0)) {
        if (rightPulses < rightPulsesGoTo) {
            rightPulses = (int)rightDirection * rightQei->getPulses();
            rightVelocity = (rightPulses - rightPrevPulses) / RATE;
            rightPrevPulses = rightPulses;
            t=rightPid->compute();
            if(((rightPulsesGoTo-rightPulses)/rightVelocity)<1.0){              
                rightPid->reset();
                rightPid->setSetPoint(t*2000);
            }
            rightPid->setProcessValue(fabs(rightVelocity));
            rightMotor->speed(rightDirection * t);
        } else {
            rightPulsesGoTo=0;
            this->StopRight();
        }
    } else {
        rightPulsesGoTo=0;
        this->StopRight();

    }
    if ((leftPulsesGoTo!=0)&&(leftSpeed>0)) {
        if (leftPulses < leftPulsesGoTo) {
            leftPulses =  (int)leftDirection * leftQei->getPulses();
            leftVelocity = (leftPulses - leftPrevPulses) / RATE;
            leftPrevPulses = leftPulses;
            t=leftPid->compute();            
            if(((leftPulsesGoTo-leftPulses)/leftVelocity)<1.0){               
                leftPid->reset();
                leftPid->setSetPoint(t*2000);
            }
            leftPid->setProcessValue(fabs(leftVelocity));            
            leftMotor->speed(leftDirection * t);
        } else {
            leftPulsesGoTo=0;
            this->StopLeft();
        }
    } else {
        leftPulsesGoTo=0;
        this->StopLeft();
    }
}