File content as of revision 3:8a5c76437c38:

#include "mbed.h"
#include "Motors_Out.h"

Motors_Out::Motors_Out(PinName pwm1, PinName pwm2, PinName pwm3, PinName pwm4, PinName pwms, PinName en):
    DC_A1(pwm1),DC_A2(pwm2),DC_B1(pwm3),DC_B2(pwm4),SERVO(pwms),H_ENABLE(en)
{
    init();
    t.attach(this, &Motors_Out::caller, 0.02);
}

void Motors_Out::init()
{
    dir = 0;
    aim = 0;
    dir1 = 0;
    aim1 = 0;
    self_drive = false;
    on = true;
    DC_B1 = 0;          //v=0 m/s
    DC_B2 = 0;
    DC_A1.period_us(100);
    SERVO.period(0.02);
    angle = 64;
}

void Motors_Out::manualInfo(int a)
{
    self_drive = false;
    dir1 = (a & 0x6);
    aim1 = (a & 0x9);
    if(dir1 != 6) {
        dir = dir1;
        aim = aim1;
    } else {
        dir = 0;
        aim = 0;
    }
    dir1 = 0;
    aim1 = 0;
}

void Motors_Out::selfDrive(int path)
{
    self_drive = true;
    angle = path;
}

void Motors_Out::caller()
{
    if(self_drive) {
        if(angle > 10 && angle < 117) Motors_Out::autonomous();
    } else {
        Motors_Out::DCSpeed();
        if(SERVO < 0.034 || SERVO > 0.064) SERVO = 0.049;
        Motors_Out::servoAim();
    }
}

void Motors_Out::DCSpeed()
{
    DC_A1.period_us(100);                 //frequency of DC motor = 10 kHz
    switch(dir) {
        case 4:
            H_ENABLE = 1;
            DC_B2 = 0;
            DC_A2 = DC_B2;
            if(DC_A1 >= 0.3) DC_A1 = 0.3; //max speed limited 30%
            else DC_A1 = DC_A1 + 0.015;   //step change of 5%
            DC_B1=  DC_A1;
            break;
        case 2:
            H_ENABLE = 1;
            DC_B1 = 0;
            DC_A1 = DC_B1;
            if(DC_A2 >= 0.3) DC_A2 = 0.3;
            else DC_A2 = DC_A2 + 0.015;
            DC_B2 = DC_A2;
            break;
        default:
            if(DC_B1 > 0 || DC_B2 > 0) {
                DC_B1 = DC_B1 - 0.015;
                DC_B2 = DC_B2 - 0.015;
            }
            if(DC_B1 <= 0) DC_B1 = 0;
            if(DC_B2 <= 0) DC_B2 = 0;
            DC_A1 = DC_B1;
            DC_A2 = DC_B2;
            if(DC_B1 == 0 && DC_B2 == 0) H_ENABLE = 0;
    }
}

void Motors_Out::servoAim()
{
    SERVO.period(0.02);                                 //frequency of servo = 50 Hz
    if(aim == 8) {
        SERVO = SERVO - 0.0005;
        if(SERVO < 0.035) SERVO = 0.035;                //max left
    } else if(aim == 1) {
        SERVO = SERVO + 0.0005;
        if(SERVO > 0.063) SERVO = 0.063;                //max right
    } else {
        if(SERVO > 0.0515) SERVO = SERVO - 0.0005;      //step change of 2%  
         else if(SERVO < 0.0465) SERVO = SERVO + 0.0005;  
         else SERVO = 0.049;                            //forward
    }
}

void Motors_Out::autonomous()   //this function can
{                               //be much improved
    SERVO.period(0.02);         //with additional testing
    DC_A1.period_us(100);
    H_ENABLE = 1;
    DC_B2 = 0;
    DC_A2 = DC_B2;

    if(angle < 30) angle = 30;
    if(angle > 97) angle = 97;
    if(angle >= 60 && angle <= 67) serv = 0.049;                        //forward
    else if(angle < 60) serv = 0.049 - ((60 - ((float)angle))*7/15000); //left
    else if(angle > 67) serv = 0.049 + ((((float)angle) - 67)*7/15000); //right
    timeout.attach(this,&Motors_Out::servoSig,0.0015); //used to put SERVO to 0 after
    if(on) {                                           //the wanted SERVO signal is sent.
        on = false;                                    //sometimes another SERVO pulse
        SERVO = serv;                                  //would just pop up right after 
    }                                                  //the good one.
    if(angle < 40) {                            //electronic differential
        if(DC_B1 >= 0.25) DC_B1 = 0.25;         //which can be a lot better.
    } else if(angle > 87) {                     //with this basic solution
        if(DC_A1 >= 0.25) DC_A1 = 0.25;         //performance has got better
    } else if(DC_A1 >= 0.35) {
        DC_A1 = 0.35;
        DC_B1=  DC_A1;
    } else {
        DC_A1 = DC_A1 + 0.015;              //step change of 5%
        DC_B1=  DC_A1;
    }
}

void Motors_Out::servoSig()
{
    SERVO = 0;
    on = true;
}