Diff: main.cpp

Revision:

9:278d25dc0ef3

Parent:

8:fe907b9a0bab

--- a/main.cpp	Mon Oct 17 09:37:52 2016 +0000
+++ b/main.cpp	Fri Oct 28 09:58:22 2016 +0000
@@ -9,113 +9,479 @@
 
 // Setup Pins
 // Note: Pin D10 and D11 for encoder, D4-D7 for motor controller
-AnalogIn pot1(A0);
-AnalogIn pot2(A1);
+// Potmeter 1 gives the reference position x
+AnalogIn pot1(A3);
+// Potmeter 2 gives the reference position y
+AnalogIn pot2(A4);
 
 // Setup Buttons
-DigitalIn button1(D2);
-// InterruptIn button2(D3);
+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);
+DigitalOut motor2_dir(D7);
+FastPWM motor2_pwm(D6);
 
 // Set LED pins
 DigitalOut led(LED_RED);
 
 // Set HID scope
-HIDScope    scope(2);
+HIDScope    scope(6);
 
 // Set encoder
-QEI EncoderCW(D10,D11,NC,32);
-QEI EncoderCCW(D11,D10,NC,32);
+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;
+}
 
-// Variables counter
-int countsCW = 0;
-int countsCCW = 0;
-int net_counts = 0;
+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);
+}
 
-// Variables degrees, and speed
-float degrees = 0.0;
-volatile float curr_degrees = 0.0;
-volatile float prev_degrees = 0.0;
-volatile float speed = 0.0; // speed in degrees/s
+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 Calc_Prev_Theta1_Gear ();
+    double Calc_Prev_Theta2_Gear ();
+    CalcXY();
+    double Dia1 = CalcDia1 (x, y);
+    double Dia2 = CalcDia2 (x, y);
+    CalcTheta1 (Dia1);
+    CalcTheta2 (Dia2);
+    AngleLimits (Theta1Gear, Theta2Gear, Prev_x, Prev_y);       // laatste check
+    // hier mag je de motor gaan aansturen
+    //scope.send();
+}
+
+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);
+}
 
-volatile const int counts_per_rev = 8400;
-volatile const float T_CalculateSpeed = 0.001; // 1000 Hz
+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;
+}
 
-// BiqUadChain
-BiQuadChain bqc;
-BiQuad bq1( 3.72805e-09, 7.45610e-09, 3.72805e-09, -1.97115e+00, 9.71392e-01 );
-BiQuad bq2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98780e+00, 9.88050e-01 );
+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;
+}
 
 
 
-float GetReferenceVelocity()
-{
-    // Returns reference velocity in rad/s. 
-    // Positive value means clockwise rotation.
-    const float maxVelocity=8.4; // in rad/s of course!   
-    float referenceVelocity;  // in rad/s
-    float button_val = button1.read();
-    if (button1.read()) {
-        // Clockwise rotation
-        referenceVelocity = pot1.read()*maxVelocity;
-    }
-    else {
-        // Counterclockwise rotation      
-        referenceVelocity = -1*pot1.read()*maxVelocity;
-    }  
-    return referenceVelocity;
-}
-
-float FeedForwardControl(float referenceVelocity)
-{
-    // very simple linear feed-forward control
-    const float MotorGain=8.4; // unit: (rad/s) / PWM
-    float motorValue = referenceVelocity / MotorGain;
-    pc.printf("\r\n RefVel = %f \r\n",motorValue); 
-    return motorValue;
-}
-
-void SetMotor1(float motorValue)
+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 (motorValue >=0){
-        motor1_dir=1;
-    }
-    else{
-        motor1_dir=0;
-        pc.printf("\r\n MOTORDIR = 0 \r\n");
-    }
-    if (fabs(motorValue)>1){
-        motor1_pwm.write(1);
+    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{
-        motor1_pwm.write(fabs(motorValue));
-    }
-}
-
-void MeasureAndControl(void)
-{
-    // This function measures the potmeter position, extracts a
-    // reference velocity from it, and controls the motor with 
-    // a simple FeedForward controller. Call this from a Ticker.
-    float referenceVelocity = GetReferenceVelocity();
-    float motorValue = FeedForwardControl(referenceVelocity);
-    SetMotor1(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();
@@ -132,6 +498,7 @@
     scope.set(1,speed_filtered);
     scope.send();
 }
+*/
 
 int main(){
     // Set baud connection with PC
@@ -145,16 +512,47 @@
     
     // Set motor PWM speeds
     motor1_pwm.period(1.0/1000);
-    // motor2_pwm.period(1.0/1000);
+    motor2_pwm.period(1.0/1000);
     
-    Ticker CalculateSpeedTicker;
-    CalculateSpeedTicker.attach(&CalculateSpeed,T_CalculateSpeed);
+    // 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);
     
-    // Setup Biquad
-    bqc.add(&bq1).add(&bq2);
+    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);
     
-    // MeasureAndControl as fast as possible
-    while(true){
-        MeasureAndControl();
-    }
+    // 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){}
 }
\ No newline at end of file