File content as of revision 2:2ad485d762f5:

#include "AnglePosition.h"
#include "PIDControl.h"
#include "mbed.h"
#include "encoder.h"
#include "Servo.h"
#include "MODSERIAL.h"
#include "FastPWM.h"

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
MODSERIAL pc(USBTX, USBRX);         //
Ticker MyControllerTicker1;         //Declare Ticker Motor 1
Ticker MyControllerTicker2;         //Declare Ticker Motor 2
AnglePosition Angle;                //Declare Angle calculater
PIDControl PID;                     //Declare PID Controller
AnalogIn potmeter1(A3);             //Set Inputpin
AnalogIn potmeter2(A4);             //Set Inputpin

const float CONTROLLER_TS = 0.02;   //Motor frequency

//------------------------------------------------------------------------------
//-------------------------Kinematic Constants----------------------------------
//------------------------------------------------------------------------------
const double RAD_PER_PULSE = 0.00074799825*2;
const double PI = 3.14159265358979323846;
const float max_rangex = 500.0;
const float max_rangey = 300.0;
const float x_offset = 156.15;
const float y_offset = -76.97;
const float alpha_offset = -(21.52/180)*PI;
const float beta_offset  = 0.0;
const float L1 = 450.0;
const float L2 = 490.0;


//------------------------------------------------------------------------------
//--------------------------------Servo-----------------------------------------
//------------------------------------------------------------------------------
Servo MyServo(D9);                  //Declare button
InterruptIn But1(D8);               //Declare used button
int k=0;                            //Position flag

void servo_control (){
    if (k==0){
        MyServo = 0;
        k=1;
    }
    else{
        MyServo = 2;
        k=0;
    }
}


//------------------------------------------------------------------------------
//--------------------------------Motor1----------------------------------------
//------------------------------------------------------------------------------
FastPWM motor1(D5);
DigitalOut motor1DirectionPin(D4);
DigitalIn ENC2A(D12);
DigitalIn ENC2B(D13);
Encoder encoder1(D13,D12);
const float MOTOR1_KP = 40.0;
const float MOTOR1_KI = 0.0;
const float MOTOR1_KD = 15.0;
double M1_v1 = 0.0;
double M1_v2 = 0.0;
const double motor1_gain = 2*PI;
const float M1_N = 0.5;


void motor1_control(){
    float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, potmeter1, potmeter2);
    float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
    float error_alpha = reference_alpha-position_alpha;
    float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain;
    motor1 = fabs(magnitude1);
    pc.printf("err_a = %f  alpha = %f   mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1);
    pc.printf("\r\n");
    // Determine Motor Direction 
    if (magnitude1  < 0){
        motor1DirectionPin = 1;
    }
    else{
        motor1DirectionPin = 0;
    }
}

//------------------------------------------------------------------------------
//--------------------------------Motor2----------------------------------------
//------------------------------------------------------------------------------
FastPWM motor2(D6);
DigitalOut motor2DirectionPin(D7);
DigitalIn ENC1A(D10);
DigitalIn ENC1B(D11);
Encoder encoder2(D10,D11);
const float MOTOR2_KP = 60.0;
const float MOTOR2_KI = 0.0;
const float MOTOR2_KD = 15.0;
double m2_err_int = 0;
const double motor2_gain = 2*PI;
const float M2_N = 0.5;
double M2_v1 = 0.0;
double M2_v2 = 0.0;


void motor2_control(){
    float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, potmeter1, potmeter2);
    float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
    float error_beta = reference_beta-position_beta;
    float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain;
    motor2 = fabs(magnitude2);
    pc.printf("err2 = %f  beta = %f   mag2 = %f\r\n", error_beta, reference_beta, magnitude2);
    pc.printf("\r\n");
    //Determine Motor Direction
    if (magnitude2 > 0){
        motor2DirectionPin = 1;
    }
    else{
        motor2DirectionPin = 0;
    }
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------

int main(){
    pc.baud(115200);
    motor1.period(0.0001f);
    motor2.period(0.0001f);
    MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); 
    MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
    But1.rise(&servo_control);      
    while(1) {}   
}