File content as of revision 5:e90c8b57811c:

#include "mbed.h"
#include "Motor.h"
#include "PID.h"

#define PWM_PIN p22
#define MIN_1_PIN p19
#define MIN_2_PIN p20
#define ENC_A_PIN p16
#define ENC_B_PIN p17
#define CURRENT_PIN p18

#define pi 3.141592654

#define PWM_PERIOD 0.00005
#define PID_CUR_PERIOD 0.001
#define PID_POS_PERIOD 0.005

#define PID_POS_P_GAIN  0.5
#define PID_POS_D_GAIN  10.0
#define PID_POS_I_GAIN  0.1

#define PID_CUR_P_GAIN  0.5
#define PID_CUR_D_GAIN  0.0
//#define PID_CUR_I_GAIN  0.001
#define PID_CUR_I_GAIN  0.01

Serial pc(USBTX, USBRX);


Motor motor(
    PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD, 
    CURRENT_PIN,
    ENC_A_PIN, ENC_B_PIN, 
    PID_CUR_P_GAIN, PID_CUR_D_GAIN, PID_CUR_I_GAIN
);

HBridge hbridge(PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD);
//CurrentSense current_sense(CURRENT_PIN);
Encoder encoder(ENC_A_PIN, ENC_B_PIN);

PIDController position_controller(
    PID_POS_P_GAIN, PID_POS_D_GAIN, PID_POS_I_GAIN
);

Ticker current_ticker;

void update_current(void) {
    motor.update();
}

Ticker position_ticker;

void update_position(void) {
    motor.set_command(position_controller.command_position(motor.get_position()));
}

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);

int main() {
    wait(5);
    pc.printf("Built " __DATE__ " " __TIME__ "\r\n");
    
    led1 = 1;
    led2 = 1;
    led3 = 1;
    
    wait(5);
    
    led2 = 0;
    pc.printf("Initializing\n\r");
    
  //  current_ticker.attach(&update_current, PID_CUR_PERIOD);
   // position_ticker.attach(&update_position, PID_POS_PERIOD);
    
    int count = 0;
    float command = 0.0;
  //  wait(5);
    led3 = 0;
    pc.printf("Starting\n\r");
    
    hbridge.initialize();
    
    while(1){
      //command = 0.05 * abs((50-count)/10) - 0.1;
       //motor.set_command(command);
       command = sin(count*pi/50);
        if(command>0)
        {
            hbridge.forward();
        }
        else if(command<0)
        {
             hbridge.back();
        }
        
        led1 = (count % 2);

        
        pc.printf("Position:%d\n\r",(int) encoder.get_position());
        //printf("PC:%F\n\r", position_controller.command);
        //pc.printf("MP:%F MV:%F MCu:%F MCo:%F MO:%F MT:%F P:%F\n\r", 
         //   motor.position, motor.velocity, motor.current, motor.command, motor.output, motor.torque, position_controller.command
       //);
        
        
        count++;
        wait(0.01);
    }
}