File content as of revision 2:da3b7dd2beb0:

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

//Aray of locations
float X [10] = {-4, -3, -2, -1, 0, 1, 2, 3, 4};
float Y [10] = {-4, -3, -2, -1, 0, 1, 2, 3, 4};

//---------- 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.55;    //minimum power required to make the motor move
//Motor 2
const float M2_Ks = 0.4;           //minimum power required to move
const float M2_Kp = 0.1;          //strentgh of proportional control
const float M2_Kd = 0.1;          //strentgh of differential control Include timestep in constant
const float M2_Ki = 0.1;           //strentgh of integrational control Include timestep in constant
const double M2_friction = 0.55;    //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 cc 0=cw
PwmOut          E2(D6);   //speed 2
DigitalOut      M1(D4);  //direction 1 //1 is cc 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_dpos, M1_pos;
double M2_dpos, M2_pos;
double M_error, M1_error, M2_error;
double dR, A_R, L_error, M1_dTheta;
double position;
double pref,pref2;
bool M1_dir, M2_dir;
const double long gearbox = 0.08587786259541984732824427480916;
const double long PI = 3.14159265359;
double M_error1 = 0;

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

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

//------------------Activate programs-----------
//Activate send data pc
void D_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;
}
//read position of motor and drive motor to set position.
void M_Speed_True()
{
    speed_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();
//    pc.printf("Deg M1: %f M2: %f\n", M1_pos, M2_pos);
}

//Desired Position Motors --> future script emg
void Setpoint()
{
    //New version of setpoint which takes board into account
    //set desired x and y point in array
    int dX = pot1.read() * 10;
    int dY = pot2.read() * 10;

    //Set the length of the arm
    dR = sqrt( X[dX] * X[dX] + Y[dY] * Y[dY]);//desired length of the arm
    M1_dTheta = acos( Y[dY] / X[dX] );          //desired angle of the arm (theta for M1) //Aandacht is dit graden?!!!!!
}

void Length arm()
{

    //read out length of arm
    double A_Rx = L1 * cos( M1 * PI / 180.0) + L2 * cos( (M1 + M2)* PI / 180.0);
    double A_Ry = L1 * sin( M1 * PI / 180.0) + L2 * sin( (M1 + M2)* PI / 180.0);
    A_R = sqrt( pow (A_Rx, 2) + pow (A_Ry, 2));

    L_error = dR-A_R;
    double ThetaP = 0;
    double dTheta = ThetaR+ThetaP;//rekening houden met theta 2
    double M2_dTheta = 0;

}

void setlength(){
    L_error = dR-A_R;
    int Direction = (A_R > dR) ? false:true;
    }

double Pw_control (double Pulse,double setpoint,double Ks, double Kp,double Ki,double Kd, double friction)
{
    //read position motor
    position = gearbox*Pulse;
    // Motor Power
    M_error = abs(setpoint-position);
    
    
    
    double M_error_Int = M_error_Int+M_error/1e4;
    double Ps = Ks;
    double Pp = Kp * M_error;                                        //Proportional control
    double Pi = Ki * M_error_Int*M_error;  //check dit --> nog niet goed                            //int controll
    double Pd = Kd * (M_error-M_error1);    //Differtial control
    M_error1 = M_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'.
    if (Power<friction) {
        Power=0;
    } else {}

    //print error
//    pc.printf("Error: %f Direction: %f Position M: %f Setpoint %f \n", M_error, position, setpoint);

    pc.printf("Power: %f\n", Power);
    return Power ;//is it possible to return 2 things for
}
// direction controll
bool Dr_control (double Pulse,double setpoint)
{
    //read position motor
    double position = gearbox*Pulse;
    // Direction motor should turn
    int Direction = (position > setpoint) ? false:true;
    return Direction;
}
// begin to set point
void calibration()
{
    M1_dpos = 0;
    M2_dpos = 0;
    if (button == true) {
        M1_pos = 0;
        M2_pos = 0;
    } else {
    }
}


//--------------- 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(&D_Send_True, 1e4);         //Send data to pc
    t2.attach_us(&M_Setpoint_True, 1e4);   //Desired position motor(EMG goes here)
    t3.attach_us(M_Control_True, 1e4);      //Speed control here


//-------------Scedule programs-------------------
    while(1) {
        if (setpoint_go == true) {
            Setpoint();

            setpoint_go = false;
        }  else if (control_go == true) {
            // control Motor1
            E1 = Pw_control (Motor1.getPulses(), M1_dpos, M1_Ks, M1_Kp, M1_Ki, M1_Kd, M1_friction);
            M1 = Dr_control (Motor1.getPulses(), M1_dpos);
            M1_pos = position;
            M1_error = M_error;
            // control Motor2
            E2 = Pw_control (Motor2.getPulses(), M2_dpos, M2_Ks, M2_Kp, M2_Ki, M2_Kd, M2_friction);
            //M2 = Dr_control (Motor2.getPulses(),M2_dpos);
            M2_pos = position;
            M2_error = M_error;
            //print error
            pc.printf("M1_pos = %f M1_error = %f M2_pos = %f M2_error = %f\n", M1_pos, M1_error, M2_pos, M2_error); // once this works place it in 'send'
            control_go = false;
        } else if (send_go == true) {
            Send();

            send_go = false;
        } else {

//-----------End of scedule progrmas----------------
        }
    }
}