File content as of revision 5:ec6dd614aa7e:

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

//Aray of locations and Length of parts
float X [10] = {-0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4, 0.5};
float Y [10] = {-0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4, 0.5};
const double L1 = 0.34;
const double L2 = 0.26;
const double L_board= 0.42;
const double R_Max_error = 0.01;

//---------- 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 = -2;          //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 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);
DigitalOut      magnet(D8);         //magnet is defined on D8
MODSERIAL       pc(USBTX, USBRX);
DigitalIn       button(PTA4);
DigitalIn       button_Arm1(D1);
DigitalIn       button_Arm2(D2);
DigitalIn       button_Servo(D0);
HIDScope        scope(2);
Servo           swagvo(D3);

//Define Variables
double M1_dpos, M1_pos;
double M2_dpos, M2_pos;
double M_error, M1_error, M2_error;
double R_set, A_R, R_error, R_error_int, R_cal, R_cal1, R_cal2;
double Q_set, Q_end, Q_error, Q_error_int, Q_cal, Q_cal1, Q_cal2;
double position;
const double long gearbox = 0.085877862;
const double long PI = 3.14159265359;
double M_error1 = 0;
int run = 0;
bool H_wheel=0; // 0 is up and 1 is down
bool Magnet=0;

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

//booleans run program
volatile bool send_go = false, setpoint_go = false, control_go= false, calibration_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 Setpoint()
{
    // check if
    double R_set1=R_set;
    double Q_set1=Q_set;

    //set desired x and y point in array --> do this with EMG
    int dX = pot1.read() * 10;
    int dY = pot2.read() * 10;

    //Set the length of the arm
    R_set = sqrt( X[dX] * X[dX] + Y[dY] * Y[dY]);  //desired length of the arm
    Q_set = acos( Y[dY] / X[dX] );          //desired angle of the arm (theta for Qref) [rad]

    if (R_set1!=R_set && Q_set1 != Q_set) { // of ipv en
     pc.printf("reset\n");
        run=run+1;
        R_error_int=0;
        Q_error_int=0;
    }

}

void Lengtharm()
{
    //read out length of arm
    double A_Rx = L1 * cos(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * cos( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0);
    double A_Ry = L1 * sin(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * sin( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0);
    A_R = sqrt( pow (A_Rx, 2) + pow (A_Ry, 2)); //length Arm
    R_error= A_R-R_set; //error in arm length
}

void Qpos()
{
    //read out pos of Motor 1
    double A_Rx = L1 * cos(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * cos( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0);
    double A_Ry = L1 * sin(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * sin( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0);
    pc.printf("A_Rx = %f A_Ry = %f", A_Rx, A_Ry);
    Q_end= acos( A_Ry / A_Rx ); //actual angle of the arm  //Aandacht is dit graden(moet in radians)?!!!!!
    pc.printf("Q_set = %f Q_end= %f \n", Q_set, Q_end);
    Q_error= Q_end-Q_set; //error in angle of Qref
}

double Pw_control (double S_error, double& S_error_int, double Ks, double Kp,double Ki,double Kd, double friction)
{
    // Motor Power
    M_error = S_error; //give the amount of error

    double M_error_int = S_error_int + M_error / 1e4;
    S_error_int = M_error_int;
    double Ps = Ks;
    double Pp = Kp * M_error;                                        //Proportional control
    double Pi = Ki * M_error_int;  //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' Also limit power to a max.
    if (Power<friction) {
        Power=0;
    } else if (Power>0.7) {
        Power=0.7;
    } else {}

    return Power ;//is it possible to return 2 things for
}

// direction controll
bool dr_control (double A, double B)
{
    // Direction motor should turn
    int Direction = (A > B) ? true:false;
    return Direction;
}

void Servo()
{
    swagvo.SetPosition(650);                //lower wire
    wait(1);
    magnet = !magnet;                       //attach piece
    wait(1);
    swagvo.SetPosition(2350);               //raise wire
}

//--------------- 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);
    swagvo.Enable(1500,20000);

    pc.baud(115200);

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

//calibration
// 0 point position
    pc.printf("0 cal\n");
    bool cal0 = true;
    while (cal0 == true) {
        if (button == false) {
            Qpos();
            Lengtharm();
            Q_cal=Q_end;
            R_cal=A_R;
            cal0= false;

            //show usefull info on serial
            pc.printf("Q_set = %f Q_end= %f Q_error = %f \n", Q_set, Q_end, Q_error);
            pc.printf("R_set = %f A_R = %f R_error = %f \n", R_set, A_R, R_error);
        }
    }


    // Board point A
    wait (0.5);
    pc.printf("A cal\n");
    bool calA = true;
    while (calA == true) {
        if (button == false) {
            Qpos();
            Lengtharm();
            Q_cal1=Q_end;
            R_cal1=A_R;
            calA = false;

            //show usefull info on serial
            pc.printf("Q_set = %f Q_end= %f Q_error = %f \n", Q_set, Q_end, Q_error);
            pc.printf("R_set = %f A_R = %f R_error = %f \n", R_set, A_R, R_error);
        }
    }

    //Board point B

    wait (0.5);
    pc.printf("B cal\n");
    bool calB = true;
    while (calB == true) {
        if (button == false) {
            Qpos();
            Lengtharm();
            Q_cal2=Q_end;
            R_cal2=A_R;
            calB = false;

            //show usefull info on serial
            pc.printf("Q_set = %f Q_end= %f Q_error = %f \n", Q_set, Q_end, Q_error);
            pc.printf("R_set = %f A_R = %f R_error = %f \n", R_set, A_R, R_error);
        }
    }


//-------------Schedule programs-------------------
    while(1) {
        if (calibration_go == true) {
            pc.printf("calibration R_set = %f R_cal = %f \n", R_set, R_cal);
            //setpoint
            R_set = R_cal;
            Q_set = Q_cal;
            if (button_Arm1 == true && button_Arm2 == true ) {
                pc.printf("cal is set\n");
                Q_end = Q_cal;
                A_R = R_cal;
                calibration_go == false;
            } else {}
        } if(setpoint_go == true && calibration_go == false) {
            pc.printf("setpoint\n");
            if (run == 2) {
                pc.printf("Run = %f\n", run);
                calibration_go == true;
                run = 0;
            } else {
                Setpoint();
            }
            setpoint_go = false;
        }
        if (control_go == true) {
            pc.printf("control\n");
            Lengtharm();
            Qpos();
            if (-1*R_Max_error<R_error<R_Max_error) {
                pc.printf("Length control R_error = %f R_set = %f A_R = %f\n", R_error, R_set, A_R);
                // control Motor2
                E2 = Pw_control (R_error, R_error_int, M2_Ks, M2_Kp, M2_Ki, M2_Kd, M2_friction);
                M2 = dr_control (A_R,R_set);
                M2_error = M_error;
                //print error
                pc.printf("R_error =  %f\n", R_error);
            } else if (-1*R_Max_error<Q_error<R_Max_error) {
                pc.printf("Angle control Q_error = %f Q_set = %f Q_end = %f\n", Q_error, Q_set, Q_end);
                // control Motor 1
                E1 = 0;//Pw_control (Q_error, Q_error_int, M1_Ks, M1_Kp, M1_Ki, M1_Kd, M1_friction);
                M1 = dr_control (Q_end, Q_set);
                // control Motor 2 - has to do nothing
                E2 = 0;
                // print error
                pc.printf("Q_error =  %f\n", Q_error);
            } else {
                E1 = 0;
                E2 = 0;
                pc.printf("i dont know what to do R_error = %f R_set = %f A_R = %f\n", R_error, R_set, A_R);
            }
            control_go = false;
        }
        if (send_go == true) {
            pc.printf("send\n");
            Send();

            send_go = false;
        }
        if (button_Servo == false) {
            pc.printf("servo\n");
            Servo();
        }

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

    }
}