Diff: main.cpp

Revision:

3:8caef4872b0c

Parent:

2:94b5e00288a5

Child:

4:19e376d31380

--- a/main.cpp	Fri Oct 14 14:37:25 2016 +0000
+++ b/main.cpp	Mon Oct 17 10:39:56 2016 +0000
@@ -10,7 +10,7 @@
 DigitalIn encoder1B (D12); //Channel B van Encoder 1
 DigitalOut led1 (D11); 
 DigitalOut led2 (D10);
-AnalogIn potMeter1(A0);
+AnalogIn potMeter1(A2);
 AnalogIn potMeter2(A1);
 DigitalOut motor1DirectionPin(D7);
 PwmOut motor1MagnitudePin(D6);
@@ -18,26 +18,20 @@
 
 //set settings
 Serial pc(USBTX,USBRX);
-Ticker MeasurePTicker; 
-HIDScope    scope(2);
+Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
+//HIDScope    scope(2);
 
 //set datatypes
-int counts;
+int counts = 0;
 double DerivativeCounts;
 int countsPrev = 0;
 float referenceVelocity = 0;
-double bqcDerivativeCounts = 0;
-const double PI = 3.141592653589793;
-double Potmeter1 = potMeter1.read();
-double Potmeter2 = potMeter2.read();
-const int cw = 1;
-const int ccw = 0;
-
-//define encoder counts and degrees
-QEI Encoder(D12, D13, NC, 32); // turns on encoder
-int counts_per_revolution = 4200;
-const double gear_ratio = 131;
-const double resolution = counts_per_revolution/(2*PI/gear_ratio);  //counts per radian
+float bqcDerivativeCounts = 0;
+const float PI = 3.141592653589793;
+//float Potmeter1 = potMeter1.read();
+//float Potmeter2 = potMeter2.read();
+const int cw = 0;       //values for cw and ccw are inverted!! cw=0 and ccw=1
+const int ccw = 1;
 
 //set BiQuad
 BiQuadChain bqc;
@@ -45,9 +39,107 @@
 BiQuad bq2(1.0000,   -1.5704,    1.2756,   -0.4844,    0.0762);
 
 //set go-Ticker settings
-volatile bool MeasurePTicker_go=false; 
-void MeasureP_act(){MeasurePTicker_go=true;}; // Activates go-flags
+volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
+void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
+void BiQuadTicker_act(){BiQuadTicker_go=true;};
+void FeedbackTicker_act(){FeedbackTicker_go=true;};
+void TimeTracker_act(){TimeTracker_go=true;};
+//void sampleT_act(){sampleT_go=true;};
+
+//define encoder counts and degrees
+QEI Encoder(D12, D13, NC, 32); // turns on encoder
+const int counts_per_revolution = 4200;
+const int gear_ratio = 131;
+const float resolution = counts_per_revolution/(2*PI/gear_ratio);  //counts per radian
+
+float GetReferencePosition()
+{
+    // Returns reference position in rad. 
+    // Positive value means clockwise rotation.
+    const float maxPosition = 2*PI; //6.283185307179586; // in radians
+    float Potmeter1 = potMeter1.read();
+    float referencePosition = Potmeter1 * maxPosition; //Potmeter1 * maxPosition; //refpos in radians 
+    pc.printf("Max Position: %f rad \r\n", maxPosition);
+    //pc.printf("Potmeter1, refpos: %f \r\n", Potmeter1);
+    pc.printf("Ref Position: %f rad \r\n", referencePosition);
+    return referencePosition;
+}
+
+float FeedForwardControl(float referencePosition)
+{
+    //QEI Encoder(D13, D12, NC, 32); // turns on encoder
+    //int counts = Encoder.getPulses();           // gives position of encoder 
+    float Position = counts/resolution;         //position in radians
+    // linear feedback control
+    float ControllerInput = referencePosition - Position; // 'error' in radians
+    float Kp = potMeter2.read(); //Potmeter2;
+    float motorValue = ControllerInput * Kp;
+    pc.printf("Position: %f rad \r\n", Position);
+    pc.printf("Counts: %i rad \r\n", counts);
+    pc.printf("Kp: %f \r\n", Kp);
+    pc.printf("MotorValue: %f \r\n", motorValue);
+    return motorValue;
+}
 
+void SetMotor1(float motorValue)
+{
+    // 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
+    if (motorValue >=0) 
+        {motor1DirectionPin=cw;
+        led1=1;
+        led2=0;
+        }
+    else {motor1DirectionPin=ccw;
+        led1=0;
+        led2=1;
+        }
+    if (fabs(motorValue)>1) motor1MagnitudePin = 1;
+        else motor1MagnitudePin = fabs(motorValue);
+}
+
+void MeasureAndControl()
+{
+    // This function measures the potmeter position, extracts a
+    // reference position from it, and controls the motor with 
+    // a Feedback controller. Call this from a Ticker.
+    float referencePosition = GetReferencePosition();
+    float motorValue = FeedForwardControl(referencePosition);
+    SetMotor1(motorValue);
+}
+
+void TimeTrackerF(){
+     //wait(1);   
+     //float Potmeter1 = potMeter1.read();
+     float referencePosition = GetReferencePosition();
+     pc.printf("TTReference Position: %d rad \r\n", referencePosition);
+     //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
+     //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
+     pc.printf("TTCounts: %i \r\n", counts);
+}
+/*
+void sample()
+{
+    int countsPrev = 0;
+    QEI Encoder(D12, D13, NC, 32);
+    counts = Encoder.getPulses();  // gives position
+    //scope.set(0,counts);
+    DerivativeCounts = (counts-countsPrev)/0.001;
+    //scope.set(1,DerivativeCounts);
+    countsPrev = counts; 
+    //scope.send();
+    pc.printf("Counts: %i rad/s \r\n", counts);
+    pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts);
+}
+
+void BiQuadFilter(){            //this function creates a BiQuad filter for the DerivativeCounts
+    //double in=DerivativeCounts();
+    bqcDerivativeCounts=bqc.step(DerivativeCounts);
+    //return(bqcDerivativeCounts);
+    }
+    
 void MeasureP(){
     double ref_position = Potmeter1;                //reference position from potmeter
     int counts = Encoder.getPulses();           // gives position    
@@ -55,39 +147,82 @@
     double rotation = ref_position-position;     //rotation is 'position error' in radians
     double movement = rotation/(2*PI);  //movement in rotations
     double Kp = Potmeter2;
-
+    }
+    
+double P(double rotation, double Kp){
     double P_output = Kp*movement;
+    return P_output;
+    }
+    
+void MotorController(){
+    double output = P(rotation, Kp);
     
     if(rotation>0){
         motor1DirectionPin.write(cw);
-        motor1MagnitudePin.write(P_output);
+        motor1MagnitudePin.write(output);
         }
     if(rotation<0){
         motor1DirectionPin.write(ccw);
-        motor1MagnitudePin.write(-(P_output));
+        motor1MagnitudePin.write(-output);
         }
-    pc.printf("ref_position: %d rad/s \r\n", ref_position);
-    pc.printf("position: %d rad \r\n", position);
-    pc.printf("rotation: %d rad \r\n", rotation);
-    pc.printf("Kp: %d \r\n", Kp);
     }
-
+*/
 int main()
 {
  //Initialize
- led1=0;
- led2=0;
- //float Potmeter1 = potMeter1.read();
- //float Potmeter2 = potMeter2.read();
- MeasurePTicker.attach(&MeasureP_act, 0.01f); 
- pc.baud(115200);  
+ led1=1;
+ led2=1;
+ pc.baud(115200);
+ pc.printf("Test putty");
+ //float Potmeter = potMeterIn.read();
+ MeasureTicker.attach(&MeasureTicker_act, 1.0f); 
+ bqc.add(&bq1).add(&bq2);
+ //BiQuadTicker.attach(&BiQuadTicker_act, 0.01f); //frequentie van 100 Hz
+ //TimeTracker.attach(&TimeTracker_act, 1.0f);
  QEI Encoder(D12, D13, NC, 32); // turns on encoder
+ //sampleT.attach(&sampleT_act, 0.1f);
+ //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
+ //pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
  
  while(1)
     {
-        if (MeasurePTicker_go){
-            MeasurePTicker_go=false;
-            MeasureP();
+        if (MeasureTicker_go){
+            MeasureTicker_go=false;
+            MeasureAndControl();
+            counts = Encoder.getPulses();           // gives position of encoder 
+            pc.printf("Resolution: %f pulses/rad \r\n",resolution);
+            }
+      /*
+            // Encoder part
+            counts = Encoder.getPulses();  // gives position
+            DerivativeCounts = ((double) counts-countsPrev)/0.01;  
+            
+            scope.set(0,counts);
+            scope.set(1,DerivativeCounts);
+            //scope.set(1,bqcDerivativeCounts);
+            scope.send();
+            countsPrev = counts;
+            //pc.printf("Counts: %i rad/s \r\n", counts);
+            //pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts);
         }
+        
+        if (BiQuadTicker_go){
+            BiQuadTicker_go=false;
+            BiQuadFilter();
+        }
+    
+        if (FeedbackTicker_go){
+            FeedbackTicker_go=false;
+            Feedback();
+        
+        if (TimeTracker_go){
+            TimeTracker_go=false;
+            TimeTrackerF();
+        }
+        
+        if (sampleT_go){
+            sampleT_go=false;
+            sample();
+        }*/
     }
 }
\ No newline at end of file