Diff: main.cpp

Revision:

16:2f89d6e25782

Parent:

15:1cfe58aea10d

Child:

17:dbdbd1edc260

--- a/main.cpp	Thu Nov 02 09:10:41 2017 +0000
+++ b/main.cpp	Thu Nov 02 13:43:01 2017 +0000
@@ -25,16 +25,28 @@
 //globalvariables Motor
 Ticker Treecko;             //We make a awesome ticker for our control system
 Ticker printer;
-//PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
-//DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control
+PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
+PwmOut M2E(D5);
+DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control
+Encoder motor1(D13,D12,true);
+Encoder motor2(D9,D8,true);
+DigitalOut M2D(D4);
 
-//Encoder motor1(D13,D12,true);
+//DEMO
+AnalogIn potMeter2(A1);
+AnalogIn potMeter1(A2);
+
+
+
 MODSERIAL pc(USBTX,USBRX);
 
-//double PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
+double PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
 const double Ts = 0.1;                   // tickettijd/ sample time
-//double e_prev = 0; 
-//double e_int = 0;
+double e_prev = 0; 
+double e_int = 0;
+double e_prev2 = 0;
+double e_int2 = 0;
+
 double tijdstap = 0.002;
 volatile double LBF;
 volatile double RBF;
@@ -108,8 +120,6 @@
 BiQuad LP2RT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
 BiQuadChain LPFRT;
 
-
-
 Timer LooptimeEMG; //moetuiteindelijk weg
 
 //filters
@@ -141,10 +151,10 @@
 AnalogIn emgLT(A2);
 AnalogIn emgRT(A3);
 
-//float MVCLB = 0.3;
-//float MVCRB = 0.3;
-//float MVCLT = 0.3;
-//float MVCRT = 0.3;
+//double MVCLB = 0.3;
+//double MVCRB = 0.3;
+//double MVCLT = 0.3;
+//double MVCRT = 0.3;
 
 // variabelen changePosition
 int goalx, goaly;
@@ -294,7 +304,110 @@
                                          //PAS ALS DEZE TRUE IS, MOET DE MOTOR PAS BEWEGEN!!!
   //return maxi;
 }
-       
+
+double GetReferencePosition() 
+{
+    double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden
+    int maxwaarde = 4096;                   // = 64x64
+    double refP = Potmeterwaarde*maxwaarde;
+    return refP;                            // value between 0 and 4096 
+}
+
+double GetReferencePosition2() 
+{
+    double potmeterwaarde2 = potMeter1.read();
+    int maxwaarde2 = 4096;                   // = 64x64
+    double refP2 = potmeterwaarde2*maxwaarde2;
+    return refP2;                            // value between 0 and 4096 
+}
+    
+double FeedBackControl(double error, double &e_prev, double &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
+{
+    double kp = 0.001;                             // kind of scaled.
+    double Proportional= kp*error;
+    
+    double kd = 0.0004;                           // kind of scaled. 
+    double VelocityError = (error - e_prev)/Ts; 
+    double Derivative = kd*VelocityError;
+    e_prev = error;
+    
+    double ki = 0.00005;                           // kind of scaled.
+    e_int = e_int+Ts*error;
+    double Integrator = ki*e_int;
+    
+    double motorValue = Proportional + Integrator + Derivative;
+    return motorValue;
+}
+
+double FeedBackControl2(double error2, double &e_prev2, double &e_int2)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
+{
+    double kp2 = 0.001;                             // kind of scaled.
+    double Proportional2= kp2*error2;
+    
+    double kd2 = 0.0004;                           // kind of scaled. 
+    double VelocityError2 = (error2 - e_prev2)/Ts; 
+    double Derivative2 = kd2*VelocityError2;
+    e_prev2 = error2;
+    
+    double ki2 = 0.00005;                           // kind of scaled.
+    e_int2 = e_int2+Ts*error2;
+    double Integrator2 = ki2*e_int2;
+    
+    double motorValue2 = Proportional2 + Integrator2 + Derivative2;
+    return motorValue2;
+}
+
+
+void SetMotor1(double 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(double 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
+    }
+}
+
+void MeasureAndControl(void)
+{
+    // hier the control of the 1st control system
+    double refP = GetReferencePosition();                    //KOMT UIT RKI
+    double Huidigepositie = motor1.getPosition(); 
+    double error = (refP - Huidigepositie);// make an error
+    double motorValue = FeedBackControl(error, e_prev, e_int);
+    SetMotor1(motorValue);
+    // hier the control of the 2nd control system
+    double refP2 = GetReferencePosition2(); 
+    double Huidigepositie2 = motor2.getPosition(); 
+    double error2 = (refP2 - Huidigepositie2);// make an error
+    double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+    SetMotor2(motorValue2);
+}
+     
 void changePosition ()    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
 {
     if (RBF>0.3) {
@@ -312,90 +425,19 @@
     pc.printf("goalx = %i, goaly = %i\r\n",goalx, goaly);
 } 
 
-/*
-double Encoder ()
-{
-    double Huidigepositie = motor1.getPosition ();
-    return Huidigepositie;             // huidige positie = current position
-}
-
-double FeedBackControl(double error, double &e_prev, double &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
-{
-    double kp = 0.001;                           // has jet to be scaled
-    double Proportional= kp*error;
-    
-    double kd = 0.0004;                            // has jet to be scaled
-    double VelocityError = (error - e_prev)/Ts; 
-    double Derivative = kd*VelocityError;
-    e_prev = error;
-    
-    double ki = 0.00005;                           // has jet to be scaled 
-    e_int = e_int+Ts*error;
-    double Integrator = ki*e_int;
-    
-    
-    double motorValue = Proportional + Integrator + Derivative;
-    return motorValue;
-}
-
-void SetMotor1(double 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 MeasureAndControl ()
-{
-        // hier the control of the control system
-     
-    if(caldone==false)
-    {
-       if(button1.read()==false)  
-        {
-            CalibrationEMG();
-       }
-    }
-    if (caldone==true)
-    
-    {
-       Filteren();
-       changePosition();
-        //rest
-    }
- */ 
-    //double Huidigepositie = Encoder(); 
-    //double error = (refP - Huidigepositie);// make an error
-    //double motorValue = FeedBackControl(error, e_prev, e_int);
-    //double motorValue = refP;
-    //SetMotor1(motorValue);
-//}
 void Loop_funtion()
 {   
     switch(State){
         case Cal1: //Calibration motor 1
         // naar achteren bewegen( als voorbeeld Arvid), daarna deze waarde opslaan als offset. Dan bewegen naar home middels PID en verschil encodervalue uiterste stand en home1.
-            motorValue1 = 0.5f; motorValue2=0;
-        
-        
+            motorValue1 = 0.1f; motorValue2=0;
+            M2E = fabs(motorValue2);
+            M1E = fabs(motorValue1);
+            if         
                if (Huidigepositie1== 0)
                {
-                    SetMotor1(value); //value is waarde encoder voor loodrechte hoeken,.
-                    if (fabs(Huidigepositie1-home1)<0.01) {
+                                        SetMotor1(value); //value is waarde encoder voor loodrechte hoeken,.
+                    if (fabs(Huidigepositie1-)<0.01) {
                         State=Cal2;
                     }
                 }
@@ -431,6 +473,8 @@
         case EMG: //Aansturen met EMG
             Filteren();
             changePosition();
+            //RKI --> output refP van motor
+            MeasureAndControl();
             break;
          case Demo: // Aansturen met toetsenbord
             break;
@@ -486,9 +530,7 @@
     Position_controller_on = false;
     Treecko.attach(&Loop_funtion, looptime);
     while(true)
-    {
-        
-        }
+    {   }
     
-} //is deze wel nodig?  
-}
\ No newline at end of file
+ //is deze wel nodig?  
+ }