Diff: main.cpp

Revision:

0:ec8fa8a84edd

Child:

1:e3db171abbb2

diff -r 000000000000 -r ec8fa8a84edd main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 01 16:45:26 2017 +0000
@@ -0,0 +1,230 @@
+//libaries
+#include "mbed.h"
+#include "encoder.h"
+#include "MODSERIAL.h"
+
+
+// globale variables
+Ticker AInTicker;           //We make a ticker named AIn (use for HIDScope)
+
+Ticker Treecko;             //We make a awesome ticker for our control system
+AnalogIn potMeter2(A1);     //Analoge input of potmeter 2 (will be use for te reference position)
+PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
+DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control
+
+Encoder motor1(D13,D12,true);
+MODSERIAL pc(USBTX,USBRX);
+
+float PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
+const float Ts = 1/500;                   // tickettijd/ sample time
+float e_prev = 0; 
+float e_int = 0;
+float PwmPeriod2 = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
+float e_prev2 = 0; 
+float e_int2 = 0;
+
+double pi = 3.14159265359;
+double SetPx = 38;     //Setpoint position x-coordinate from changePosition (EMG dependent)
+double SetPy = 30;     //Setpoint position y-coordinate from changePosition (EMG dependent)
+volatile double q1 = 0;          //Reference position q1 from calibration (only the first time)
+volatile double q2 = pi/2;       //Reference position q2 from calibration (only the first time)
+const double L1 = 30;  //Length arm 1
+const double L2 = 38;  //Length arm 2
+double K = 1;           //Spring constant for movement end-joint to setpoint
+double B1 = 1;          //Friction coefficient for motor 1
+double B2 = 1;          //Friction coefficient for motot 2
+double T = 1/500;       //Desired time step
+double Motor1Set;       //Motor1 angle
+double Motor2Set;       //Motor2 angle
+double p;
+
+
+//tweede motor
+AnalogIn potMeter1(A2);
+PwmOut M2E(D5);
+DigitalOut M2D(D4);
+Encoder motor2(D9,D8,true);
+
+void RKI()
+{
+    p=sin(SetPx);
+    q1 = q1 + ((sin(q1)*L1 + sin(q2)*L2)*SetPy - (cos(q1)*L1 + cos(q2)*L2)*SetPx)*(K*T)/B1;     //Calculate desired joint 1 position
+    q2 = q2 + ((SetPy - cos(q1)*L1)*sin(q2)*L2 + (sin(q1)*L1 - SetPx)*cos(q2)*L2)*(K*T)/B2;     //Calculate desired joint 2 position
+    
+    Motor1Set = q1/(2*pi);           //Calculate the desired motor1 angle from the desired joint positions
+    Motor2Set = (pi-q2-q1)/(2*pi);   //Calculate the desired motor2 angle from the desired joint positions
+    
+    //pc.printf("waarde p = %f \r\n",p);
+    //pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set);
+    //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy);
+}
+
+void SetpointRobot()
+{   
+    double Potmeterwaarde2 = potMeter2.read();
+    double Potmeterwaarde1 = potMeter1.read();
+
+    if (Potmeterwaarde2>0.5) {
+        SetPx++;    // hoe veel verder gaat hij? 1 cm? 10 cm?
+    }
+    if (Potmeterwaarde2<0.5) {
+        SetPx--;
+    }
+    if (Potmeterwaarde1>0.5) {
+        SetPy++;
+    }
+    if (Potmeterwaarde1<0.5) {
+        SetPy--;
+    }
+    //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy);
+}
+
+float GetReferencePosition() 
+{
+    float Potmeterwaarde = potMeter2.read();
+    int maxwaarde = 4096;                   // = 64x64
+    float refP = Potmeterwaarde*maxwaarde;
+    return refP;                            // value between 0 and 4096 
+}
+
+float GetReferencePosition2() 
+{
+    float potmeterwaarde2 = potMeter1.read();
+    int maxwaarde2 = 4096;                   // = 64x64
+    float refP2 = potmeterwaarde2*maxwaarde2;
+    return refP2;                            // value between 0 and 4096 
+}
+    
+float FeedBackControl(float error, float &e_prev, float &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
+{
+    float kp = 0.001;                             // kind of scaled.
+    float Proportional= kp*error;
+    
+    float kd = 0.0004;                           // kind of scaled. 
+    float VelocityError = (error - e_prev)/Ts; 
+    float Derivative = kd*VelocityError;
+    e_prev = error;
+    
+    float ki = 0.00005;                           // kind of scaled.
+    e_int = e_int+Ts*error;
+    float Integrator = ki*e_int;
+    
+    
+    float motorValue = Proportional + Integrator + Derivative;
+    return motorValue;
+}
+
+float FeedBackControl2(float error2, float &e_prev2, float &e_int2)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
+{
+    float kp2 = 0.001;                             // kind of scaled.
+    float Proportional2= kp2*error2;
+    
+    float kd2 = 0.0004;                           // kind of scaled. 
+    float VelocityError2 = (error2 - e_prev2)/Ts; 
+    float Derivative2 = kd2*VelocityError2;
+    e_prev2 = error2;
+    
+    float ki2 = 0.00005;                           // kind of scaled.
+    e_int2 = e_int2+Ts*error2;
+    float Integrator2 = ki2*e_int2;
+    
+    
+    float motorValue2 = Proportional2 + Integrator2 + Derivative2;
+    return motorValue2;
+}
+
+
+void SetMotor1(float motorValue)
+{
+    if (motorValue >= 0)
+    {
+        M1D = 0;
+    }
+    else 
+    {
+        M1D = 1;
+    }
+
+    if  (fabs(motorValue) > 1)    
+    {
+        M1E = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
+    }
+    else
+    {    
+        M1E = fabs(motorValue);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
+    }
+}
+
+void SetMotor2(float motorValue2)
+{
+    if (motorValue2 >= 0)
+    {
+        M2D = 0;
+    }
+    else 
+    {
+        M2D = 1;
+    }
+
+    if  (fabs(motorValue2) > 1)    
+    {
+        M2E = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
+    }
+    else
+    {    
+        M2E = fabs(motorValue2);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
+    }
+}
+
+float Encoder ()
+{
+    float Huidigepositie = motor1.getPosition ();
+    return Huidigepositie;             // huidige positie = current position
+}
+
+float Encoder2 ()
+{
+    float Huidigepositie2 = motor2.getPosition ();
+    return Huidigepositie2;             // huidige positie = current position
+}
+
+void MeasureAndControl(void)
+{
+    // RKI aanroepen
+    SetpointRobot();
+    RKI();
+    
+    // hier the control of the control system
+    //float refP = GetReferencePosition(); 
+    float Huidigepositie = Encoder(); 
+    float error = (Motor1Set - Huidigepositie);// make an error
+    float motorValue = FeedBackControl(error, e_prev, e_int);
+    SetMotor1(motorValue);
+
+    // hier the control of the control system
+    //float refP2 = GetReferencePosition2(); 
+    float Huidigepositie2 = Encoder2(); 
+    float error2 = (Motor2Set - Huidigepositie2);// make an error
+    float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+    SetMotor2(motorValue2);
+}
+
+
+int main()
+{
+    M1E.period(PwmPeriod);
+    Treecko.attach(&MeasureAndControl, Ts);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
+                                            //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
+      pc.baud(115200);
+     
+     
+    while(1) 
+    {
+        wait(0.2);
+        float B = motor1.getPosition();
+        float Potmeterwaarde = potMeter2.read();
+        //float positie = B%4096;
+        //pc.printf("pos: %d, speed %f, potmeter = %f V, \r\n",motor1.getPosition(), motor1.getSpeed(),(potMeter2.read()*3.3)); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V
+        pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f, Setpointx = %f, Setpointy = %f \r\n", q1, q2, Motor1Set, Motor2Set, SetPx, SetPy);   
+    }
+}