File content as of revision 12:4310fb637705:

#include "controls.h"

void dir_control_M1(controls dir)
{
    switch(dir) {
        case forward:
            M1F=1;
            M1B=0;
            break;
        case backward:
            M1F=0;
            M1B=1;
            break;
        case brake:
            M1F=1;
            M1B=1;
            break;
        case stall:
        default:
            M1F=0;
            M1B=0;
            break;
    }
}
void dir_control_M2(controls dir)
{
    switch(dir) {
        case forward:
            M2F=1;
            M2B=0;
            break;
        case backward:
            M2F=0;
            M2B=1;
            break;
        case brake:
            M2F=1;
            M2B=1;
            break;
        case stall:
        default:
            M2F=0;
            M2B=0;
            break;
    }
}

const double pulse_cycle = 10; // ms

void move(double cycle_speed, int duration_ms, controls dir_M1, controls dir_M2, double (*PID_error)())   // cycle_speed from 0 - 1
{
    Timer T;
    Timer T_M1;
    Timer T_M2;

    //start moving first
    bool High_M1=true;
    bool High_M2=true;

    double pulse_M1;
    double period_M1;
    double pulse_M2;
    double period_M2;

    double cycle_speed_M1;
    double cycle_speed_M2;

    dir_control_M1(dir_M1);
    dir_control_M2(dir_M2);

    T.start();
    T_M1.start();
    T_M2.start();

    while(T.read_ms()<(duration_ms)) {

        cycle_speed_M1= cycle_speed - (*PID_error)(); //PID_error() should be under 1 most/all of the time ideally
        cycle_speed_M2= cycle_speed + (*PID_error)();

        if(cycle_speed_M1>1) // Can't go over 1 as that is the max speed
            cycle_speed_M1=1;
        if(cycle_speed_M1<0) { //Can't go under 0 either
            //  pc.printf("M1: %8.8f\n",cycle_speed_M1);
            cycle_speed_M1=0;
        }
        if(cycle_speed_M2>1)
            cycle_speed_M2=1;
        if(cycle_speed_M2<0) {
            //pc.printf("M2: %8.8f\n",cycle_speed_M2);
            cycle_speed_M2=0;
        }

        //pc.printf("m1: %8.8f\n",cycle_speed_M1);
        //pc.printf("m2: %8.8f\n",cycle_speed_M2);
        //adjusting delays based off PID_error corrections
        pulse_M1 = (pulse_cycle-pulse_cycle*(1-cycle_speed_M1));
        period_M1 = (pulse_cycle-pulse_cycle*cycle_speed_M1);


        pulse_M2 = (pulse_cycle-pulse_cycle*(1-cycle_speed_M2));
        period_M2 = (pulse_cycle-pulse_cycle*cycle_speed_M2);

        //pc.printf("pulse2: %8.8f\n", pulse_M1);
        // pc.printf("period2: %8.8f\n", period_M1);
        //controlling the speed for motor 1
        if(High_M1) {
            High_M1=pulse_M1 > T_M1.read_ms();
            //pc.printf("timer m1 move: %8.8f\n",T_M1.read_ms());
            if(!High_M1) { //pulse has finished and going LOW
                dir_control_M1(stall);
                pc.printf("timer m1 stall: %f\n",T_M1.read_ms());
                T_M1.reset();  //start the delay between pulse (note that it continues counting)
            }
        } else {
            High_M1=period_M1 < T_M1.read_ms();
            if(High_M1) { //delay has finished and going HIGH
                dir_control_M1(dir_M1);
                T_M1.reset(); //start the pulse and its clock
            }
        }

        //repeated for Motor 2
        if(High_M2) {
            High_M2=pulse_M2 > T_M2.read_ms();
            if(!High_M2) {
                dir_control_M2(stall);
                T_M2.reset();
            }
        } else {
            High_M2=period_M2 < T_M2.read_ms();
            if(High_M2) {
                dir_control_M2(dir_M2);
                T_M2.reset();
            }
        }

    }
    T.stop();
    T_M1.stop();
    T_M2.stop();

}

void sharp_right(int delay, int cycle_speed)
{
    move(cycle_speed, delay, forward, backward, PID);
    move(cycle_speed, delay, forward, backward, PID);
}
void sharp_left(int delay)
{

}