Diff: main.cpp

Revision:

7:05495acc08b0

Parent:

6:083bd713670b

Child:

8:42d1d02673ae

Child:

12:e125b9fa77b9

--- a/main.cpp	Tue Oct 31 15:44:56 2017 +0000
+++ b/main.cpp	Wed Nov 01 14:59:42 2017 +0000
@@ -21,6 +21,17 @@
 float e_prev = 0; 
 float e_int = 0;
 
+//tweede motor
+AnalogIn potmeter1(A2);
+PwmOut M2E(D5);
+DigitalOut M2D(D4);
+Encoder motor2(D9,D8,true);
+Ticker DubbelTreecko;
+
+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;
+
 
 
 float GetReferencePosition() 
@@ -30,6 +41,14 @@
     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)
 {
@@ -50,6 +69,26 @@
     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)
@@ -71,12 +110,39 @@
     }
 }
 
+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)
 {
     // hier the control of the control system
@@ -87,13 +153,23 @@
     SetMotor1(motorValue);
 }
 
+void MeasureAndControl2(void)
+{
+    // hier the control of the control system
+    float refP2 = GetReferencePosition2(); 
+    float Huidigepositie2 = Encoder2(); 
+    float error2 = (refP2 - 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.
-     
+    DubbelTreecko.attach(MeasureAndControl2, Ts);
      
      
     while(1)