File content as of revision 5:d3bdc0672891:

#include "mbed.h"
#include "HBridgeDCMotor.h"
#include "FastPWM.h"
#include "AutomationElements.h"

DigitalIn fault(PTE20);
DigitalIn sw1(PTC13);
DigitalIn sw2(PTC17);
AnalogIn a_feedback(PTE23);
AnalogIn b_feedback(PTE22);
AnalogIn pot1(PTB3);
AnalogIn pot2(PTB2);

Serial pc(PTA2, PTA1);


DigitalOut enable(PTE21);  
DigitalOut d1(PTB8); 
DigitalOut d2(PTB9);
DigitalOut d3(PTB10);
DigitalOut d4(PTB11);
//DigitalOut red(PTB18);
//DigitalOut green(PTB19);
//DigitalOut blue(PTD1);

//HBridgeDCMotor motor_a(PTC3, PTC4);
//HBridgeDCMotor motor_b(PTC1, PTC2);

PwmOut servo(PTB0);

FastPWM motor1(PTC3);
DigitalOut motor1_grana2(PTC4);
FastPWM motor2(PTC1);
DigitalOut motor2_grana2(PTC2);

Ticker ticker;
float u_R_a = 0.3, u_R_b = 0.3, x=0;
double u_i_a = 0, u_i_b = 0;
double T_d = 1e-3;

PI PI_a (0.04, 0.001, T_d);
PI PI_b (0.04, 0.001, T_d);

void tick(){
    u_i_a = 375.0/220.0*(3.3 * a_feedback)/u_R_a;
    u_i_b = 375.0/220.0*(3.3 * b_feedback)/u_R_b;
    x = 3 * pot2;
    PI_a.in(x - u_i_a);
    PI_b.in(x - u_i_b);
    u_R_a = PI_a.out();
    u_R_b = PI_b.out();
    
    motor1.pulsewidth(100e-6*u_R_a);
    motor2.pulsewidth(100e-6*u_R_b); 

    }

void steerServo(float angleDegrees){   
    float minPw = 0.0005, maxPw = 0.0025, minAng = -90, maxAng = 90, pulse = 0;
    if (angleDegrees < -29 ) angleDegrees = -29;
    if (angleDegrees > 30 ) angleDegrees = 30;
    pulse = minPw + ((maxPw - minPw)/(maxAng - minAng))*(angleDegrees - minAng);
    servo.pulsewidth(pulse);
    }

int main() {
    float pw =0, PWmin=-90, PWmax=90;
    float angleDegrees = 0;
    float sampleTime = 50e-3, switchingFrequency = 5000, rampTime = 3;
    servo.period(1.0/50);
    PI_a.setOutputLimits (0.3, 1);
    PI_b.setOutputLimits (0.3, 1);

    steerServo(angleDegrees);
    motor1.period_us(100);
    motor2.period_us(100);
    
  //  motor_a.configure(sampleTime, switchingFrequency, rampTime, rampTime);
   // motor_b.configure(sampleTime, switchingFrequency, rampTime, rampTime);
    wait(5);
    enable = 1;
    motor2_grana2 = 0;
    motor1_grana2 = 0;
    ticker.attach(&tick, T_d);

    while(1){

        
  //      angleDegrees = PWmin + (PWmax - PWmin) * pot1;

       // if(sw1==1 && sw2==0) enable = sw1;
      //  else enable = 0;
      
        
        

        wait(1);
        
        
   /**     motor2_grana2 = 0;


        motor2.pulsewidth_us(100*u_R);
           
       steerServo(angleDegrees);
     //  pc.printf("pot1: %f   angleDegrees: %f pot2: %f\r", x, angleDegrees, u_R);
        
    //   motor_a.setDutyCycle(0.2);
     //  motor_b.setDutyCycle(0.2); 
        
        if(x<0.501 && x>0.499) {d1=1; d2=1; d3=1; d4=1;}
        else {d1=1; d2=0; d3=0; d4=1;}      
**/
        }
    //enable = 0;
   // red = 1;
    

}