File content as of revision 5:19687a179088:

#include "mbed.h"
#include "encoder.h"
#include "MODSERIAL.h"

/** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
void keep_in_range(float * in, float min, float max);

volatile bool looptimerflag;

void setlooptimerflag(void)
{
    looptimerflag = true;
}


int main() {
    //LOCAL VARIABLES 
    AnalogIn potmeter(PTC2);
    MODSERIAL pc(USBTX,USBRX);

    Encoder motor1(PTD0,PTC9);//first pin on PTAx or PTDx
    Encoder motor2(PTD5,PTC8);//first pin on PTAx or PTDx
    PwmOut pwm_motor1(PTA12);
    PwmOut pwm_motor2(PTA5);
    DigitalOut motordir1(PTD3);
    DigitalOut motordir2(PTD1);
    
    pwm_motor1.period(1.0/22000.0);
    
//MOTOR A    
    float setpoint;
    float pwm_to_motor1;
    float setspeed;
//    float speed;
//    float position2;
//    float setpoint2;
//MOTOR B    
    float setpointB;
    float pwm_to_motor2;
    float setspeedB;
    float speedB;
    float position2B;
    float setpoint2B;
    
    //START OF CODE
    pc.baud(115200);
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,0.001);  
    //A
//    speed = 0; 
//    position2 = 0;
//    setpoint = 0;
    //B
    speedB = 0; 
    position2B = 0;
    setpoint2B = 0;
    //INFINITE LOOP 
    while(1) {
        while(looptimerflag != true);
        looptimerflag = false;
        
//MOTOR A
        setspeed = (potmeter.read()-0.5)*.0001;
        setpoint = setpoint + setspeed;
        pwm_to_motor1 = (setpoint - (motor1.getPosition()/4128))*20 + (setspeed - (motor1.getSpeed()/4128))*1.4 ;
        keep_in_range(&pwm_to_motor1, -1,1);
        
        if(pwm_to_motor1 > 0)
            motordir1 = 1;
        else
            motordir1 = 0;
        //WRITE VALUE TO MOTOR  
        pwm_motor1.write(abs(pwm_to_motor1));
        
//MOTOR B
        setpointB = (potmeter.read()-0.5)*8000;
        setspeedB =(setpointB - setpoint2B)/0.01;
        speedB = (motor2.getPosition() - position2B)/0.001;
        pc.printf("s: %f, %d \n\r", setpointB, motor2.getPosition());
        pwm_to_motor2 = (setpointB - motor2.getPosition())*.0001 + (setspeedB - speedB)*.00005 ;
        keep_in_range(&pwm_to_motor2, -1,1);
        setpoint2B = setpointB;
        position2B = motor2.getPosition();
        if(pwm_to_motor2 > 0)
            motordir2 = 1;
        else
            motordir2 = 0;
        //WRITE VALUE TO MOTOR  
        pwm_motor2.write(abs(pwm_to_motor2));
    }
}


//coerces value 'in' to min or max when exceeding those values
//if you'd like to understand the statement below take a google for
//'ternary operators'.
void keep_in_range(float * in, float min, float max)
{
    *in > min ? *in < max? : *in = max: *in = min;
}