File content as of revision 6:05b6b70618db:

#include "mbed.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "QEI.h"

//---------- Change motor control parameters-------
//Motor 1
const float M1_Ks = 0.4;           //minimum power required to move
const float M1_Kp = 0.1;          //strentgh of proportional control
const float M1_Kd = 0.1;          //strentgh of differential control Include timestep in constant
const float M1_Ki = 0.1;           //strentgh of integrational control Include timestep in constant
const double M1_friction = 0.4;    //minimum power required to make the motor move
//Motor 2
const float M2_Ks = 0;           //minimum power required to move
const float M2_Kp = -10;          //strentgh of proportional control
const float M2_Kd = 0;          //strentgh of differential control Include timestep in constant
const float M2_Ki = 1;           //strentgh of integrational control Include timestep in constant
const double M2_friction = 0.4;    //minimum power required to make the motor move
//----------End of control parameters

//encoder
QEI Motor1 (D13, D12, NC, 32);
QEI Motor2 (D11, D10, NC, 32);

//Define pins
DigitalOut      M2(D7);             //direction 2 //1 is ccw 0=cw
PwmOut          E2(D6);             //speed 2
DigitalOut      M1(D4);             //direction 1 //1 is ccw 0=cw
PwmOut          E1(D5);             //speed 1
AnalogIn        pot1(A0);           //read value of pot1 for position
AnalogIn        pot2(A1);           //read value of pot2 for position
DigitalOut      myled(LED_GREEN);
MODSERIAL       pc(USBTX, USBRX);
DigitalIn       button(PTA4);
HIDScope        scope(2);

//Define Variables
double M1_set, M1_error, M1_pos, M1_error_int, M1_error1;
double M2_set, M2_error, M2_pos, M2_error_int, M2_error1;
double position;
const double long gearbox = 0.085877862;


//Timers and Tickers
Timer t;
Ticker t1,t2,t3;

//booleans run program
volatile bool send_go = false, setpoint_go = false, control_go= false;

//------------------Activate programs-----------
//Activate send data pc
void Send_True()
{
    send_go = true;
}
// Activate desired location
void M_Setpoint_True()
{
    setpoint_go = true;
}
// Controll if motor should go or not
void M_Control_True()
{
    control_go = true;
}
//------------------End of activate programs--------


//------------------Start of control programs-------
//Send values over HIDScope & Serial port
void Send()
{
    scope.set(0,Motor1.getPulses());
    scope.set(1,Motor2.getPulses());
    scope.send();
}

//Desired Position Motors
void M_Setpoint()
{
    M1_set=pot1.read()*360-180;
    M2_set=pot2.read()*360-180;
}

// read position of motors
void M_pos ()
{
    M1_pos = gearbox * Motor1.getPulses();
    M2_pos = gearbox * Motor1.getPulses();
}

// calculate error between pos and setpoint
void M_error ()
{
    M1_error= M1_set - M1_pos;
    M2_error= M2_set - M2_pos;
}

//translate error to power
double Pw_control (double& S_error, double& S_error_int, double S_error1, double Ks, double Kp,double Ki,double Kd, double friction)
{
    // Motor Power
    S_error_int = S_error_int + S_error / 1e4;
    double Ps = Ks;
    double Pp = Kp * S_error;                 //Proportional control
    double Pi = Ki * S_error_int;             //int controll
    double Pd = Kd * (S_error - S_error1);    //Differtial control
    S_error1 = S_error;
    double Power = Ps + Pp + Pi + Pd;
    pc.printf("Power bf: %f Pi: %f \n", Power, Pi);

    // overcome minimum power required to turn and stop the motor from 'piepen' Also limit power to a max.
    if (Power<friction) {
        Power=0;
    } else if (Power>0.7) {
        Power=0.7;
    } else {}

    return Power ;
}

// direction control
bool dr_control (double A, double B)
{
    int Direction = (A > B) ? false:true;
    return Direction;
}
//--------------- End of control programs----------

int main()
{
    //turn that led off!It hurts my eyes! Ow, I do boot.
    myled = 1;

    //PWM period motors
    E1.period(0.0001f);
    E2.period(0.0001f);
    pc.baud(115200);

    //sub programs - time how fast everything occurs
    t1.attach_us(&Send_True, 1e4);         //Send data to pc
    t2.attach_us(&M_Setpoint_True, 1e4);   //Desired position motor
    t3.attach_us(M_Control_True, 1e4);      //Speed control here

//-------------Schedule programs-------------------
    while(1) {

        if(setpoint_go == true) {
            M_Setpoint();
            pc.printf("setpoint\n");

            setpoint_go = false;
        }
        if (control_go == true) {
            pc.printf("control:");
            M_pos();
            M_error();
            //control Motor 1
            E1 = Pw_control (M1_error, M1_error_int, M1_error1, M1_Ks, M1_Kp, M1_Ki, M1_Kd, M1_friction);
            M1 = dr_control (M1_pos, M1_set);
            pc.printf("M1_power: %f M1_ Direction: ", E1, M1);
            // control Motor 2
            E2 = Pw_control (M2_error, M2_error_int, M2_error1, M2_Ks, M2_Kp, M2_Ki, M2_Kd, M2_friction);
            M2 = dr_control (M2_pos, M2_set);
            pc.printf("M2_power: %f M2_ Direction:", E2, M2);
            pc.printf("M1_error: %f M2_ error: \n", M1_error, M2_error);
            control_go = false;
        }
        if (send_go == true) {
            pc.printf("send\n");
            Send();

            send_go = false;
        }
//-----------End of scedule progrmas----------------
    }
}