File content as of revision 10:65f63a0a6b3c:

#include "mbed.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include "QEI.h"
#include "BiQuad.h"
#define SERIAL_BAUD 115200  // baud rate for serial communication

Serial pc(USBTX,USBRX);

// Setup Pins
// Note: Pin D10 and D11 for encoder, D4-D7 for motor controller
// Potmeter 1 gives the reference position x
AnalogIn pot1(A3);
// Potmeter 2 gives the reference position y
AnalogIn pot2(A4);

// Setup Buttons
InterruptIn button1(PTB9);                  // button 1
InterruptIn button2(PTA1);                  // button 2
InterruptIn button3(PTC6);                 // SW2
InterruptIn button4(PTA4);                 // SW3

//
volatile bool button1_value = false;

// Set motor Pinouts
DigitalOut motor1_dir(D4);
FastPWM motor1_pwm(D5);
DigitalOut motor2_dir(D7);
FastPWM motor2_pwm(D6);

// Set LED pins
DigitalOut led(LED_RED);

// Set HID scope
HIDScope    scope(6);

// Set encoder
QEI m1_EncoderCW(D10,D11,NC,32);
QEI m1_EncoderCCW(D11,D10,NC,32);
QEI m2_EncoderCW(D13,D12,NC,32);
QEI m2_EncoderCCW(D12,D13,NC,32);

volatile const int   COUNTS_PER_REV = 8400;
volatile const float DEGREES_PER_PULSE = 8400 / 360;
volatile const float CONTROLLER_TS = 0.01; // 1000 Hz

// Set initial Kp and Ki
volatile double Kp = 0.01; // last known good Kp: 0.01
volatile double Ki = 0.0;   // last known good Ki: 0.0025
volatile double Kd = 0.0; // last known good Kd: 0.0

volatile double Ts = 0.01;
volatile double N = 100;

// Memory values for controllers
double m1_v1 = 0, m1_v2 = 0;
double m2_v1 = 0, m2_v2 = 0;

// Set default mode
volatile int Mode = 0;

// Variabelen voor MotorHoekBerekenen
volatile double x = 0.0;                             // beginpositie x en y
volatile double y = 305.5;
volatile const double pi = 3.14159265359; 
volatile double Theta1Gear = 60.0;              // Beginpositie op 60 graden
volatile double Theta2Gear = 60.0;
volatile double Theta1 = Theta1Gear*42/12;           // Beginpositie van MotorHoek
volatile double Theta2 = Theta2Gear*42/12;            // frequentie in Hz waarmee theta wordt uigereken

// Functions for angle calculation
double Calc_Prev_x () {
    double Prev_x = x;
        //pc.printf("prev x = %f\r\n", Prev_x);
    return Prev_x;
}

// vorige y opslaan
double Calc_Prev_y () {
    double Prev_y = y;
        //pc.printf("prev y = %f\r\n", Prev_y);
    return Prev_y;
}

// vorige Theta1Gear opslaan
double Calc_Prev_Theta1_Gear () {
    double Prev_Theta1_Gear = Theta1Gear;
    return Prev_Theta1_Gear;
}

//vorige Theta2Gear opslaan
double Calc_Prev_Theta2_Gear () {
    double Prev_Theta2_Gear = Theta2Gear;
    return Prev_Theta2_Gear;
}

void CalcXY()
{
    /*
    double Step = 5/Fr_Speed_Theta ;  //10 mm per seconde afleggen
    
    if (BicepsLeft==true && BicepsRight==true && Leg==true && Stepper_State==false) {   
        Stepper_State = true;     // we zijn aan het flipper dus geen andere dingen doen
        FunctieFlipper() ;
    }
    else if (BicepsLeft==true && BicepsRight==false && Leg==false && Stepper_State==false) {
        x = x - Step;
    }
    else if (BicepsLeft==false && BicepsRight==true && Leg==false && Stepper_State==false) {
        x = x + Step;       // naar Right bewegen
    }
    else if (BicepsLeft==true && BicepsRight==true && Leg==false && Stepper_State==false) {
        y = y + Step;        // naar voren bewegen
    }
    else if (BicepsLeft==false && BicepsRight==true && Leg==true && Stepper_State==false) {
        y = y - Step;       // naar achter bewegen
    }
    else if (BicepsLeft==false && BicepsRight==false && Leg==false || Stepper_State==true) {
    }
    */
    
    x = (pot1.read()-0.5f)*400.0f; // x is een waarde tussen de -200 en 200
    y = 50.0f+(pot2.read()*256.0f);
    
    // Grenswaardes     LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
    if (x > 200) {
        x = 200;
    }
    else if (x < -200) {
        x = -200;
    }
    if (y > 306) {
        y = 306;
    }
    else if (y < 50) {
        y = 50;         // GOKJE, UITPROBEREN
    }
    //pc.printf("x = %f, y = %f\r\n", x, y);
      
    //scope.set(2,x);
    //scope.set(3,y);
}    

// diagonaal berekenen voor linker arm
double CalcDia1 (double x, double y) {
    double a = 50.0;                                // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden)   KEERTJE NAMETEN
    double BV1 = sqrt(pow((a+x),2) + pow(y,2));     // diagonaal (afstand van armas tot locatie) berekenen
    double Dia1 = pow(BV1,2)/(2*BV1);               // berekenen van de afstand oorsprong tot diagonaal
    
    //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia1, x, y);
    return Dia1;
}

// diagonaal berekenen voor rechter arm
double CalcDia2 (double x, double y) {
    double a = 50.0;                                
    double BV2 = sqrt(pow((x-a),2) + pow(y,2));     // zelfde nog een keer doen maar nu voor de rechter arm 
    double Dia2 = pow(BV2,2)/(2*BV2); 
    
    //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia2, x, y);
    return Dia2;
}

// calculate Theta1
void CalcTheta1 (double Dia1) {
    double a = 50.0; 
    double Bar = 200.0;    // lengte van de armen 
    
    // Hoek berekenen van het grote tandwiel (gear)
    if (x > -a) {
        Theta1Gear = pi - atan(y/(x+a)) - acos(Dia1/Bar);
    }
    else if (x > -a) {
        Theta1Gear = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
    }
    else {  // als x=-a
        Theta1Gear = 0.5*pi - acos(Dia1/Bar);
    }
    Theta1Gear = Theta1Gear*180.0/pi;       // veranderen van radialen naar graden
             
    // omrekenen van grote tandwiel naar motortandwiel
    Theta1 = Theta1Gear*42.0/12.0;  // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12. 
        
    // pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta1, Theta1Gear);
}

void CalcTheta2 (double Dia2) {
    double a = 50.0; 
    double Bar = 200.0;    // lengte van de armen 
    double Prev_Theta2_Gear = Theta2Gear; 
        
    // Hoek berekenen van het grote tandwiel (gear)
    if (x < a) {
        Theta2Gear = pi + atan(y/(x-a)) - acos(Dia2/Bar);
    }
    else if (x > a) {
        Theta2Gear = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
    }
    else {  // als x=a
        Theta2Gear = 0.5*pi - acos(Dia2/Bar);
    }
    Theta2Gear = Theta2Gear*180/pi;         // veranderen van radialen naar graden
    
    // omrekenen van grote tandwiel naar motortandwiel
    Theta2 = Theta2Gear*42.0/12.0;  // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.

    // pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
}

// als een van de hoeken te groot wordt, zet dan alles terug naar de vorige positie
void AngleLimits (double Prev_Theta1_Gear, double Prev_Theta2_Gear, double Prev_x, double Prev_y) {
    double MaxThetaGear = 100.0;   // de hoek voordat arm het opstakel raakt (max hoek is 107.62 tussen arm en opstakel, maken er 100 van voor veiligheid)
    
    if (Theta1Gear >= MaxThetaGear || Theta2Gear >= MaxThetaGear) {
        Theta1Gear = Prev_Theta1_Gear;
        Theta2Gear = Prev_Theta2_Gear;
        x = Prev_x;
        y = Prev_y;
        
        Theta1 = Theta1Gear*42.0/12.0;
        Theta2 = Theta2Gear*42.0/12.0; 
    }
    // pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
}

void CalculationsForTheta () {
    double Prev_x = Calc_Prev_x ();
    double Prev_y = Calc_Prev_y ();
    double Prev_Theta1_Gear = Calc_Prev_Theta1_Gear ();
    double Prev_Theta2_Gear = Calc_Prev_Theta2_Gear ();
    CalcXY();
    double Dia1 = CalcDia1 (x, y);
    double Dia2 = CalcDia2 (x, y);
    CalcTheta1 (Dia1);
    CalcTheta2 (Dia2);
    AngleLimits (Prev_Theta1_Gear, Prev_Theta2_Gear, Prev_x, Prev_y);       // laatste check
}

void Set_Mode()
{
    Mode++;
    if (Mode == 3)
    {
        Mode = 0;
    }
    pc.printf("\r 0: Kp \r\n");
    pc.printf("\r 1: Ki \r\n");
    pc.printf("\r 2: Kd \r\n");
    pc.printf("\r MODE = %i \r\n",Mode);
}

void increase()
{
    switch(Mode)
    {
        case 0:
            Kp = Kp + 0.01f;
            break;
        case 1:
            Ki = Ki + 0.00001f;
            break;
        case 2:
            Kd = Kd + 0.01f;
            break;
        default:
            pc.printf("\r swtich error \r\n");
    }
    pc.printf("\r Kp: %f, Ki: %f, Kd: %f \r\n",Kp,Ki,Kd);
}

void decrease()
{
    switch(Mode)
    {
        case 0:
            if (Kp <= 0.0f)
            {
                Kp = 0.0f;
                break;
            }
            Kp = Kp - 0.01f;
            break;
        case 1:
            if (Ki == 0.0f)
            {
                Ki = 0.0f;
                break;
            }
            Ki = Ki - 0.00001f;
            break;
        case 2:
            if (Kd == 0.0f)
            {
                Kd = 0.0f;
                break;
            }
            Kd = Kd - 0.01f;
            break;
        default:
            pc.printf("\r swtich error \r\n");
    }
    pc.printf("\r Kp: %f, Ki: %f, Kd: %f \r\n",Kp,Ki,Kd);
}

double m1_GetPosition()
{
    int countsCW = m1_EncoderCW.getPulses();
    int countsCCW= m1_EncoderCCW.getPulses();
    int net_counts=countsCW-countsCCW;
    double Position=(net_counts*360.0)/COUNTS_PER_REV;
    return Position;
}

double m2_GetPosition()
{
    int countsCW = m2_EncoderCW.getPulses();
    int countsCCW= m2_EncoderCCW.getPulses();
    int net_counts=countsCW-countsCCW;
    double Position=(net_counts*360.0)/COUNTS_PER_REV;
    return Position;
}

double m1_GetPosition_cal()
{
    int countsCW = m1_EncoderCW.getPulses();
    int countsCCW= m1_EncoderCCW.getPulses();
    int net_counts=countsCW-countsCCW;
    double Position=(net_counts*360.0)/COUNTS_PER_REV+210.0f; // calibrated position is 210 degrees
    return Position;
}

double m2_GetPosition_cal()
{
    int countsCW = m2_EncoderCW.getPulses();
    int countsCCW= m2_EncoderCCW.getPulses();
    int net_counts=countsCW-countsCCW;
    double Position=(net_counts*360.0)/COUNTS_PER_REV+210.0f; // calibrated position is 210 degrees
    return Position;
}
double m1_PID(double error, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &m1_v1, double &m1_v2)
{
    double a1 = -4/(N*Ts+2),
    a2 = -1*(N*Ts - 2)/(N*Ts+2), 
    b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
    b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
    b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
    
    double v = error - a1*m1_v1 - a2*m1_v2;
    double u = b0*v + b1*m1_v1 + b2*m1_v2;
    m1_v2 = m1_v1; m1_v1 = v;
    return u;
}

double m2_PID(double error, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &m2_v1, double &m2_v2)
{
    double a1 = -4/(N*Ts+2),
    a2 = -1*(N*Ts - 2)/(N*Ts+2), 
    b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
    b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
    b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
    
    double v = error - a1*m1_v1 - a2*m1_v2;
    double u = b0*v + b1*m1_v1 + b2*m1_v2;
    m1_v2 = m1_v1; m1_v1 = v;
    return u;
}



void SetMotor(int motor_number, double MotorValue)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // clockwise. motorValues outside range are truncated to
    // within range
    if (motor_number == 1)
    {
        if (MotorValue >=0)
        {
            motor1_dir=0;
        }
        else
        {
            motor1_dir=1;
        }
        if (fabs(MotorValue)>1){
            motor1_pwm.write(1);
        }
        else
        {
            motor1_pwm.write(abs(MotorValue));
        }
    }
    else
    {  
        if (MotorValue >=0)
        {
            motor2_dir=0;
        }
        else
        {
            motor2_dir=1;
        }
        if (fabs(MotorValue)>1){
            motor2_pwm.write(1);
        }
        else
        {
            motor2_pwm.write(abs(MotorValue));
        }
    }   
}

void BlinkLed(){
    led = not led;
}

void Controller_motor()
{
    // calculate the reference position
    CalculationsForTheta();
    // get the actual position
    double m1_Position = m1_GetPosition_cal();
    double m2_Position = m2_GetPosition_cal();
    // Set position scopes
    scope.set(0,x);
    scope.set(1,y);
    scope.set(2,Theta1);
    scope.set(3,Theta2);
    /*
    scope.set(0,Theta1);
    scope.set(1,m1_Position);
    scope.set(3,Theta2);
    scope.set(4,m2_Position);
    */
    //scope.set(2,m1_Position);
    //scope.set(4,m2_Position);
    // calc the error
    double m1_error = Theta1 - m1_Position;
    double m2_error = Theta2 - m2_Position;
    //scope.set(2,m1_error);
    //scope.set(6,m2_error);
    // calc motorvalues for controller;
    double m1_MotorValue = m1_PID(m1_error, Kp, Ki, Kd, Ts, N, m1_v1, m1_v2);
    double m2_MotorValue = m2_PID(m2_error, Kp, Ki, Kd, Ts, N, m2_v1, m2_v2);
    scope.set(4,m1_MotorValue);
    scope.set(5,m2_MotorValue);
    // Set the motorvalues
    SetMotor(1, m1_MotorValue);
    SetMotor(2, m2_MotorValue);
    // Set motorvalues for scope
    // Send data to HIDScope
    scope.send();
}

void button1_switch()
{
    button1_value = not button1_value;
}

void PotControl()
{
    double Motor1_Value = (pot1.read() - 0.5f)/2.0f;
    double Motor2_Value = (pot2.read() - 0.5f)/2.0f;
    //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
    //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
    double m1_Position = m1_GetPosition();
    double m2_Position = m2_GetPosition();
    
    
    scope.set(0, Motor1_Value);
    scope.set(1, m1_Position);
    scope.set(2, Motor2_Value);
    scope.set(3, m2_Position);
    scope.send();
    // Write the motors
    SetMotor(1, Motor1_Value);
    SetMotor(2, Motor2_Value);
}
 
/*
void CalculateSpeed() {
    countsCW = EncoderCW.getPulses();
    countsCCW= EncoderCCW.getPulses();
    net_counts=countsCW-countsCCW;
    degrees=(net_counts*360.0)/counts_per_rev;
    
    curr_degrees = degrees;
    speed = (curr_degrees-prev_degrees)/T_CalculateSpeed; 
    prev_degrees = curr_degrees;
    
    //scope.set(0, degrees);
    scope.set(0, speed);
    double speed_filtered = bqc.step(speed);
    scope.set(1,speed_filtered);
    scope.send();
}
*/

int main(){
    // Set baud connection with PC
    pc.baud(SERIAL_BAUD);
    pc.printf("\r\n ***THERMONUCLEAR WARFARE COMMENCES*** \r\n");
    
    // Setup Blinking LED
    led = 1;
    Ticker TickerBlinkLed;
    TickerBlinkLed.attach(BlinkLed,0.5);
    
    // Set motor PWM speeds
    motor1_pwm.period(1.0/1000);
    motor2_pwm.period(1.0/1000);
    
    // Setup Interruptin.fall
    button1.fall(button1_switch);
    button2.fall(Set_Mode);
    button3.fall(increase);
    button4.fall(decrease);
    // Setup motor control
    Ticker PIDControllerTicker;
    Ticker PotControlTicker;
    
    pc.printf("\r\n ***************** \r\n");
    pc.printf("\r\n Press button SW3 to start positioning the arms using PotControl\r\n");
    pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r\n");
    pc.printf("\r\n ***************** \r\n");
    while (button1_value == false){}
    PotControlTicker.attach(&PotControl,0.01f);
    
    pc.printf("\r\n ***************** \r\n");
    pc.printf("\r\n When done positioning, press button SW3 to detach Potcontrol");
    pc.printf("\r\n ***************** \r\n");
    while (button1_value == true){}
    
    // Detach potcontrol
    PotControlTicker.detach();
    
    // Set motors to 0
    motor2_pwm.write(0);
    motor2_pwm.write(0);
    
    // Reset the encoders to set the 0 position
    m1_EncoderCW.reset();
    m1_EncoderCCW.reset();
    m2_EncoderCW.reset();
    m2_EncoderCCW.reset();
    
    pc.printf("\r\n ***************** \r\n");
    pc.printf("\r\n When done positioning, press button SW3 to start potmeter PID control");
    pc.printf("\r\n Make sure both potentiometers are positioned halfway!!! \r\n");
    pc.printf("\r\n ***************** \r\n");
    while (button1_value == false){}
    PIDControllerTicker.attach(&Controller_motor, CONTROLLER_TS); // 100 Hz
    while(true){}
}