Diff: main.cpp

Revision:

8:f5065cd04213

Parent:

7:ca880e05bb04

Child:

9:3874b23bb233

--- a/main.cpp	Tue Oct 31 13:12:24 2017 +0000
+++ b/main.cpp	Tue Oct 31 15:48:43 2017 +0000
@@ -395,6 +395,9 @@
 double          countzo;
 double          countzb;
 double          countzr;
+double          erroro;
+double          errorb;
+double          errorr;
  
 double          hcounto;
 double          dcounto;
@@ -414,6 +417,10 @@
 double          Pstx;
 double          Psty;
 double          T=0.02;//seconds
+
+double          kpo = 4;
+double          kpb = 4;
+double          kpr = 4;
  
 double          speedfactor1;
 double          speedfactor2;
@@ -456,25 +463,29 @@
 Ticker          controlmotor3; // één ticker van maken?
 Ticker          calculatedelta3; 
 Ticker          printdata3;     //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
- 
+
+/* 
 double GetReferenceVelocity1()
 {
+    //Berekenen error O, d.m.v. je gewenste positie (in kleine stapjes), je beginpositie (allereerste keer dus 1,1) en de counts van Encoder 1 
+    erroro=counto-hcounto-Encoder1.getPulses();
+    pc.printf("erroro = %.2f \n\r", erroro);
     // Returns reference velocity in rad/s. Positive value means clockwise rotation.
     double maxVelocity1=fabs(Vex+Vey); // max 8.4 in rad/s of course!       
     referenceVelocity1 = (-1)*speedfactor1 * maxVelocity1;  
                 
     if (Encoder1.getPulses() < (dcounto+10))
         {   speedfactor1 = 0.05;
-            pc.printf("Zit je hier?");
+            //pc.printf("Zit je hier?");
             if (Encoder1.getPulses() > (dcounto-10))
-                { pc.printf("kleiner");
+                { //pc.printf("kleiner");
                 speedfactor1=0;
                 }
         }
         else if ((Encoder1.getPulses()) > (dcounto-10))
         {   speedfactor1 = -0.05;
              if (Encoder1.getPulses() < (dcounto+10))
-                { pc.printf("groter");
+                { //pc.printf("groter");
                 speedfactor1=0;
                 }
         }
@@ -484,10 +495,29 @@
         }
         
     return referenceVelocity1;
-}         
- 
+    return kpo*erroro;
+}      */  
+
+double P1(double erroro, double kpo) {
+ return erroro*kpo;
+ }
+
+void MotorController1()
+{
+    double reference_o = counto-hcounto;
+    double position_o = Encoder1.getPulses();
+    motorValue1 = P1(reference_o - position_o, kpo);
+    if (motorValue1 >=0) motor1DirectionPin=1;
+        else motor1DirectionPin=0;
+    if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
+        else motor1MagnitudePin = fabs(motorValue1);
+    }
+      
+/* 
 double GetReferenceVelocity2()
 {
+    errorb=countb-hcountb-Encoder2.getPulses();
+    pc.printf("erroro = %.2f \n\r", errorb);
     // Returns reference velocity in rad/s. Positive value means clockwise rotation.
     double maxVelocity2=fabs(Vex+Vey); // max 8.4 in rad/s of course!       
     referenceVelocity2 = (-1)*speedfactor2 * maxVelocity2;  
@@ -512,10 +542,31 @@
         }
         
     return referenceVelocity2;
-} 
+    
+    return kpb*errorb;
+} */
+
+double P2(double errorb, double kpb) {
+ return errorb*kpb;
+ }
 
+void MotorController2()
+{
+    double reference_b = countb-hcountb;
+    double position_b = Encoder2.getPulses();
+    motorValue2 = P2(reference_b - position_b, kpb);
+    if (motorValue2 >=0) motor2DirectionPin=1;
+        else motor2DirectionPin=0;
+    if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
+        else motor2MagnitudePin = fabs(motorValue2);
+    }
+    
+/*
 double GetReferenceVelocity3()
 {
+    errorr=countr-hcountr-Encoder3.getPulses();
+    pc.printf("erroro = %.2f \n\r", errorb);
+    
     // Returns reference velocity in rad/s. Positive value means clockwise rotation.
     double maxVelocity3=fabs(Vex+Vey); // max 8.4 in rad/s of course!       
     referenceVelocity3 = (-1)*speedfactor3 * maxVelocity3;  
@@ -525,7 +576,7 @@
     if (Encoder3.getPulses() < (dcountr+10))
         {   speedfactor3 = 0.05;
             if (Encoder3.getPulses() > (dcountr-10))
-                { printf("Encoder waarde %i\n\r", Counts3);
+                { //printf("Encoder waarde %i\n\r", Counts3);
                 speedfactor3=0;
                 }
         }
@@ -542,9 +593,26 @@
         }
         
     return referenceVelocity3;
-} 
+    return kpr*errorr;
+} */
+
+double P3(double errorr, double kpr) {
+ return errorr*kpr;
+ }
+
+void MotorController3(){
+    double reference_r = countr-hcountr;
+    double position_r = Encoder3.getPulses();
+    motorValue3 = P3(reference_r - position_r, kpr);
+    if (motorValue3 >=0) motor3DirectionPin=1;
+        else motor3DirectionPin=0;
+    if (fabs(motorValue3)>1) motor3MagnitudePin = 1;
+        else motor3MagnitudePin = fabs(motorValue3);
+        }
+        
+/*    
  
-void SetMotor1(double motorValue1)
+void SetMotor1()
 {
     // 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
@@ -555,7 +623,7 @@
       
 }
  
-void SetMotor2(double motorValue2)
+void SetMotor2()
 {
     // 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
@@ -566,7 +634,7 @@
       
 }
 
-void SetMotor3(double motorValue3)
+void SetMotor3()
 {
     // 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
@@ -576,8 +644,8 @@
         else motor3MagnitudePin = fabs(motorValue3);
       
 }
- 
-double FeedForwardControl1(double referenceVelocity1)
+*/ 
+/*double FeedForwardControl1(double referenceVelocity1)
 {
     // very simple linear feed-forward control
     const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
@@ -599,34 +667,33 @@
     const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
     double motorValue3 = referenceVelocity3 / MotorGain;
     return motorValue3;  
-}
-        
+}*/
+/*        
 void MeasureAndControl1()
 {
     // 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.
-    double referenceVelocity1 = GetReferenceVelocity1();
-    double motorValue1 = FeedForwardControl1(referenceVelocity1);
-    SetMotor1(motorValue1);
+    //double referenceVelocity1 = GetReferenceVelocity1();
+    motorValue1 = MotorValue1();
+    SetMotor1();
 }
  
 void MeasureAndControl2()
 {
     // 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.
-    double referenceVelocity2 = GetReferenceVelocity2();
-    double motorValue2 = FeedForwardControl2(referenceVelocity2);
-    SetMotor2(motorValue2);
+    //double referenceVelocity2 = GetReferenceVelocity2();
+    motorValue2 = MotorValue2();
+    SetMotor2();
 }
 
 void MeasureAndControl3()
 {
     // 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.
-    double referenceVelocity3 = GetReferenceVelocity3();
-    double motorValue3 = FeedForwardControl3(referenceVelocity3);
-    SetMotor3(motorValue3);
-}
+    // and controls the motor with a simple FeedForwardouble referenceVelocity3 = GetReferenceVelocity3();
+    motorValue3 = MotorValue3();
+    SetMotor3();
+}*/
  
 void readdata1()
     {   //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
@@ -672,11 +739,11 @@
     counto=roto*4200; 
     dcounto=counto-hcounto;
     pc.printf("counto = %f \n\r", counto);
-    pc.printf("hcounto = %f \n\r", hcounto);
-    pc.printf("dcounto = %f \n\r",dcounto);
+    //pc.printf("hcounto = %f \n\r", hcounto);
+    //pc.printf("dcounto = %f \n\r",dcounto);
     countb=rotb*4200;
     dcountb=countb-hcountb;
-    pc.printf("dcountb = %f \n\r",dcountb);
+    //pc.printf("dcountb = %f \n\r",dcountb);
     countr=rotr*4200;
     dcountr=countr-hcountr;
  
@@ -690,11 +757,11 @@
     touwlengtes();
     Pex=Pstx;                  
     Pey=Psty;
-    pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty);
+    //pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty);
     //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru);
     turns();
     //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr); 
-    pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr);
+    //pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr);
     /*float R;
     R=Vex/Vey;                  // met dit stukje kan je zien dat de verhouding tussen Vex en Vey constant is en de end efector dus een rechte lijn maakt
     pc.printf("\n\r R=%f",R);*/
@@ -761,13 +828,13 @@
     pc.printf("Tiller");
     Vex = 20;
     Vey = 20;
-    controlmotor1.attach(&MeasureAndControl1, 0.01);
+    controlmotor1.attach(&MotorController1, 0.01);
     calculatedelta1.attach(&calcdelta1, 0.01);
     printdata1.attach(&readdata1, 0.5); 
-    controlmotor2.attach(&MeasureAndControl2, 0.01);
+    controlmotor2.attach(&MotorController2, 0.01);
     //calculatedelta2.attach(&calcdelta2, 0.01);
     //printdata2.attach(&readdata2, 0.5); 
-    controlmotor3.attach(&MeasureAndControl3, 0.01);
+    controlmotor3.attach(&MotorController3, 0.01);
     //calculatedelta3.attach(&calcdelta3, 0.01);
     //printdata3.attach(&readdata3, 0.5);
     }
@@ -785,7 +852,8 @@
     
     
     pc.baud(115200);
-    tiller();
+    wait(1.0f);
+    //tiller();
     getbqChain();
     threshold_timerR.attach(&Threshold_samplingBR, 0.002);
     threshold_timerL.attach(&Threshold_samplingBL, 0.002);
@@ -797,13 +865,13 @@
         tellerY();
         setcurrentposition();
         calculator();
-        controlmotor1.attach(&MeasureAndControl1, 0.01);
+        controlmotor1.attach(&MotorController1, 0.01);
         calculatedelta1.attach(&calcdelta1, 0.01);
         printdata1.attach(&readdata1, 0.5); 
-        controlmotor2.attach(&MeasureAndControl2, 0.01);
+        controlmotor2.attach(&MotorController2, 0.01);
         //calculatedelta2.attach(&calcdelta2, 0.01);
         //printdata2.attach(&readdata2, 0.5); 
-        controlmotor3.attach(&MeasureAndControl3, 0.01);
+        controlmotor3.attach(&MotorController3, 0.01);
         //calculatedelta3.attach(&calcdelta3, 0.01);
         //printdata3.attach(&readdata3, 0.5);
         //zakker();