All working, but Position_x and Position_y don't start at value 0, which causes spontaneous motor action.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_inversekin by Gerhard Berman

Files at this revision

API Documentation at this revision

Comitter:
GerhardBerman
Date:
Fri Oct 14 14:18:14 2016 +0000
Child:
1:ace33633653b
Commit message:
3 different tickers for P-controller, gives errors 'Variables undefined'

Changed in this revision

HIDScope.lib Show annotated file Show diff for this revision Revisions of this file
MODSERIAL.lib Show annotated file Show diff for this revision Revisions of this file
QEI_DCmotors.lib Show annotated file Show diff for this revision Revisions of this file
biquadFilter.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HIDScope.lib	Fri Oct 14 14:18:14 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tomlankhorst/code/HIDScope/#188304906687
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Fri Oct 14 14:18:14 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/MODSERIAL/#4737f8a5b018
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI_DCmotors.lib	Fri Oct 14 14:18:14 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/biquadFilter.lib	Fri Oct 14 14:18:14 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tomlankhorst/code/biquadFilter/#26861979d305
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 14 14:18:14 2016 +0000
@@ -0,0 +1,213 @@
+#include "mbed.h"
+#include <math.h>
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "HIDScope.h"
+#include "BiQuad.h"
+
+//set pins
+DigitalIn encoder1A (D13); //Channel A van Encoder 1
+DigitalIn encoder1B (D12); //Channel B van Encoder 1
+DigitalOut led1 (D11); 
+DigitalOut led2 (D10);
+AnalogIn potMeter1(A0);
+AnalogIn potMeter2(A1);
+DigitalOut motor1DirectionPin(D7);
+PwmOut motor1MagnitudePin(D6);
+DigitalIn button1(D5);
+
+//set settings
+Serial pc(USBTX,USBRX);
+Ticker MeasureTicker, BiQuadTicker, FeedbackTickere;// sampleT, TimeTracker;
+HIDScope    scope(2);
+
+//set datatypes
+int counts;
+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
+
+//set BiQuad
+BiQuadChain bqc;
+BiQuad bq1(0.0186,    0.0743,    0.1114,    0.0743,    0.0186); //get numbers from butter filter MATLAB
+BiQuad bq2(1.0000,   -1.5704,    1.2756,   -0.4844,    0.0762);
+
+//set go-Ticker settings
+volatile bool MeasurePTicker_go=false, PTicker_go=false, MotorControllerTicker_go=false;// TimeTracker_go=false, sampleT_go=false;
+void MeasurePTicker_act(){MeasurePTicker_go=true;}; // Activates go-flags
+void PTicker_act(){PTicker_go=true;};
+void MotorControllerTicker_act(){MotorControllerTicker_go=true;};
+//void TimeTracker_act(){TimeTracker_go=true;};
+//void sampleT_act(){sampleT_go=true;};
+
+/*
+float GetReferenceVelocity()
+{
+    // Returns reference velocity in rad/s. 
+    // Positive value means clockwise rotation.
+    const float maxVelocity = 8.4; // in rad/s of course!
+    if (button1 == 0){
+        led1=1;
+        led2=0;
+        // Counterclockwise rotation
+        referenceVelocity = potMeterIn * maxVelocity;  
+    } 
+    else {   
+        led1=0;
+        led2=1;
+        // Clockwise rotation
+        referenceVelocity = -1*potMeterIn * maxVelocity;
+    }
+    return referenceVelocity;
+}
+
+float FeedForwardControl(float referenceVelocity)
+{
+    // very simple linear feed-forward control
+    const float MotorGain=8.4; // unit: (rad/s) / PWM
+    float motorValue = referenceVelocity / MotorGain;
+    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=1;
+        else motor1DirectionPin=0;
+    if (fabs(motorValue)>1) motor1MagnitudePin = 1;
+        else motor1MagnitudePin = fabs(motorValue);
+}
+
+void MeasureAndControl()
+{
+    // 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.
+    float referenceVelocity = GetReferenceVelocity();
+    float motorValue = FeedForwardControl(referenceVelocity);
+    SetMotor1(motorValue);
+}
+
+void TimeTrackerF(){
+     wait(1);   
+     float Potmeter = potMeterIn.read();
+     pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
+     pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
+     //pc.printf("Counts: %i rad/s \r\n", counts);
+     //pc.printf("Derivative Counts: %i rad/s \r\n", DerivativeCounts);
+}
+
+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    
+    double position = counts/resolution;         //position in radians
+    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(output);
+        }
+    if(rotation<0){
+        motor1DirectionPin.write(ccw);
+        motor1MagnitudePin.write(-output);
+        }
+    }
+
+int main()
+{
+ //Initialize
+ led1=0;
+ led2=0;
+ float Potmeter = potMeterIn.read();
+ MeasureP.attach(&MeasureP_act, 0.01f); 
+ P.attach(&P_act, 0.01f);
+ MotorController.attach(&MotorController_act, 0.01f);
+ pc.baud(115200);  
+ 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 (MeasureP_go){
+            MeasureP_go=false;
+            MeasureP();
+            // 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 (PTicker_go){
+            PTicker_go=false;
+            P();
+        }
+        if (MotorControllerTicker_go){
+            MotorControllerTicker_go=false;
+            MotorController();
+        /*if (TimeTracker_go){
+            TimeTracker_go=false;
+            TimeTrackerF();
+        }
+        if (sampleT_go){
+            sampleT_go=false;
+            sample();
+        }*/
+    }
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Oct 14 14:18:14 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/aae6fcc7d9bb
\ No newline at end of file