Revision 5:6c16e9335262, committed 2017-10-19

Comitter:

vd

Date:

Thu Oct 19 12:11:53 2017 +0000

Parent:

4:983b50758735

Commit message:

Feedforward controller, set reference velocity;

Changed in this revision

diff -r 983b50758735 -r 6c16e9335262 main.cpp
--- a/main.cpp	Fri Oct 06 09:05:35 2017 +0000
+++ b/main.cpp	Thu Oct 19 12:11:53 2017 +0000
@@ -3,25 +3,25 @@
 #include "MODSERIAL.h"
 #include "math.h"
 
-
-
 DigitalOut gpo(D0);
 DigitalOut ledb(LED_BLUE);
 DigitalOut ledr(LED_RED);
 DigitalOut ledg(LED_GREEN);
-DigitalOut motor1DC(D7);
-PwmOut motor1PWM(D6);
+
+DigitalOut motor1DC(D7); 
+PwmOut motor1PWM(D6);  
 DigitalOut motor2DC(D4);
 PwmOut motor2PWM(D5);
 
-AnalogIn   potMeterIn1(A0);
-AnalogIn   potMeterIn2(A1);
+AnalogIn   potMeterIn1(A0); 
+AnalogIn   potMeterIn2(A1); 
 
 DigitalIn   button1(D11);
+DigitalIn   button2(D10); 
 
 MODSERIAL pc(USBTX,USBRX);
 QEI Encoder(D12,D13,NC,32);
-Ticker controller;
+Ticker controller; // ticker with the name controller
 
 
 float GetReferenceVelocity()
@@ -33,23 +33,60 @@
     
     float referenceVelocity;  // in rad/s
     
+    
     if (button1)   {
-        // Clockwise rotation
+        // Clockwise rotation // this should be counterclockwise (what is chosen then: CCW = positive direction)
+        // als button niet is ingedrukt:
+        // linksom = positief
+        
         referenceVelocity = potMeterIn1 * maxVelocity;
         ledr = 1;
         ledb = 0;
         } 
         else {
-        // Counterclockwise rotation
+        // Counterclockwise rotation // this should be clockwise
+        // als button wel is ingedrukt:
         referenceVelocity = -1*potMeterIn1 * maxVelocity;
         ledb = 1;
         ledr = 0;
         }
+  
+  
+   /*
+     
+   int rP1 = GetReferencePosition();
+   int counts = Encoder.getPulses();
+    
+     if (counts > rP) {
+          
+       referenceVelocity = 0*potMeterIn1 * maxVelocity;;
+       ledb = 1;
+        ledr = 0;  
+       } 
+
+*/
+
+
+/* 
+        
+    if (button2==false) {
+       
+       referenceVelocity = 0*potMeterIn1 * maxVelocity;;
+       ledb = 1;
+        ledr = 0;
+       
+       } 
+*/
+
+// check of hij stopt
+
+     
+        
     return referenceVelocity;
 }
 
 
-float FeedForwardControl(float &referenceVelocity)
+float FeedForwardControl(float &referenceVelocity) // is de motorvalue
 {
     // very simple linear feed-forward control
     const float MotorGain=8.4; // unit: (rad/s) / PWM
@@ -59,10 +96,10 @@
 
 float GetReferencePosition()
 {
-    int potmultiplier = 4200; // constant to multiply the pot 2 value with to get a reference position
-    
-    float referencePosition;
-    referencePosition = potMeterIn2 * potmultiplier;
+    int potmultiplier = 4200; // constant to multiply the pot 2 value with to get a reference position 
+    // integrated quadrature encoder that provides a resolution of 64 counts per revolution of the motor shaft, which corresponds to 8400 counts per revolution of the gearbox’s output shaft.
+    int referencePosition;
+    referencePosition = (potMeterIn2 * potmultiplier);
     return referencePosition;
 }
 
@@ -72,31 +109,68 @@
     // bits for motor 1. Positive value makes motor rotating
     // clockwise. motorValues outside range are truncated to
     // within range
-    if (motorValue >=0) motor1DC= 1;
+    if (motorValue >=0) motor1DC= 1; // direction
         else motor1DC=0;
-    if (fabs(motorValue)>1) motor1PWM = 1;
-        else motor1PWM = fabs(motorValue);
+    if (fabs(motorValue)>1) motor1PWM = 1; //??. motor1PWM is the speed
+       else motor1PWM = fabs(motorValue);
 }
 
 void MeasureAndControl(void)
 {
-    float referenceVelocity = GetReferenceVelocity();
+       
+    float referenceVelocity = GetReferenceVelocity(); // these variables are going to be ticked by 0.1
     int referencePosition = GetReferencePosition();
     int counts =  Encoder.getPulses();
     
-    
+
     
-        if(counts < referencePosition)
+       // if(counts < referencePosition)
+       
+       
         {
             float motorValue = FeedForwardControl(referenceVelocity);
             SetMotor1(motorValue);
         }
+        
+        
+/*
+        if(button2==false)
+        {
+        float motorValue = -1 * FeedForwardControl(referenceVelocity);
+            SetMotor1(motorValue);
+        }    
+                
+  if(referencePosition==1575) // dit werkt!
+        {
+        float motorValue = -1 * FeedForwardControl(referenceVelocity);
+            SetMotor1(motorValue);
+        }    
+        
+         if(referencePosition > 1575) // dit werkt!
+        {
+        float motorValue = -1 * FeedForwardControl(referenceVelocity);
+            SetMotor1(motorValue);
+        }    
+ 
+ */
+ 
+          if(counts > 1575) // dit werkt niet...
+        {
+        float motorValue = -1 * FeedForwardControl(referenceVelocity);
+            SetMotor1(motorValue);
+        }    
+ 
+
+
+        
+        /*
         else
         {
-            int b = -1;
-            float motorValue = b * FeedForwardControl(referenceVelocity);
+            float motorValue = -1 * FeedForwardControl(referenceVelocity);
             SetMotor1(motorValue);
-        }
+        }    
+*/
+ 
         
 }
 
@@ -104,25 +178,62 @@
 
 int main()
 {
-    pc.baud(115200);
+    pc.baud(9600);
     QEI Encoder(D12,D13,NC,32);
               
     ledr = 1;
     ledb = 1;
     ledg = 1;
     
-    controller.attach(&MeasureAndControl, 0.1);
+   controller.attach(&MeasureAndControl, 0.1); //ticker. Change ticker, what happens? maybe then conditions with counts work?
+//controller.attach(&MeasureAndControl, 1); // werkt niet
+//controller.attach(&MeasureAndControl, 0.01); // werkt niet
+//controller.attach(&MeasureAndControl, 0.1); // origineel
+
        
     while(1)
     {      
-        int counts = Encoder.getPulses(); 
-        float rV = GetReferenceVelocity();
-        float mV = FeedForwardControl(rV);
+        
+        float rV = GetReferenceVelocity();    
         int rP = GetReferencePosition();
+        float mV = FeedForwardControl(rV); 
+        int counts = Encoder.getPulses(); 
+ 
+ 
+/*         
+        if(counts > rP) // dit werkt niet... in de main, while loop
+        
+        {
+        float motorValue = 0 * FeedForwardControl(rV);
+            SetMotor1(motorValue);                         // hij verandert niet van richting, maar beweegt heen en weer
+        }    
+  
+*/
+
+        if(button2 == false) // counts > rP && 
+        
+   {
+       const float maxVelocity=8.4; // in rad/s of course!
+       rV = 0*potMeterIn1 * maxVelocity;;
+       ledb = 1;
+        ledr = 0;
+        
           
-        pc.printf("\r reference velocity: %f. Reference Position: %i Motor Value is: %f number of counts: %i\n",mV,rP,rV,counts);
+        float motorValue = 0 * FeedForwardControl(rV);
+            SetMotor1(motorValue);                         // hij verandert niet van richting, maar beweegt heen en weer
+        
+       } 
+
+
+
+          
+        wait(0.2) ; 
+        pc.printf("\r reference velocity: %0.2f. Reference Position: %i Motor Value is: %0.2f number of counts: %i\n",rV,rP,mV,counts);
+
     }
+     
    
 }
 
+// Voor Serial gebruik: Mac Terminal: screen /dev/tty.usbmodem1422 9600