Gerhard Berman / Mbed 2 deprecated prog_inversekin

First trial for Inverse Kinematics Feedforward implementation. No errors, not yet tested with board

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_pract3_3_PI_controller by Gerhard Berman

main.cpp@0:43160ef59f9f, 2016-10-14 (annotated)

Committer:
GerhardBerman
Date:
Fri Oct 14 14:18:14 2016 +0000
Revision:
0:43160ef59f9f
Child:
1:ace33633653b
3 different tickers for P-controller, gives errors 'Variables undefined'

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GerhardBerman 0:43160ef59f9f 1 #include "mbed.h"
GerhardBerman 0:43160ef59f9f 2 #include <math.h>
GerhardBerman 0:43160ef59f9f 3 #include "MODSERIAL.h"
GerhardBerman 0:43160ef59f9f 4 #include "QEI.h"
GerhardBerman 0:43160ef59f9f 5 #include "HIDScope.h"
GerhardBerman 0:43160ef59f9f 6 #include "BiQuad.h"
GerhardBerman 0:43160ef59f9f 7
GerhardBerman 0:43160ef59f9f 8 //set pins
GerhardBerman 0:43160ef59f9f 9 DigitalIn encoder1A (D13); //Channel A van Encoder 1
GerhardBerman 0:43160ef59f9f 10 DigitalIn encoder1B (D12); //Channel B van Encoder 1
GerhardBerman 0:43160ef59f9f 11 DigitalOut led1 (D11);
GerhardBerman 0:43160ef59f9f 12 DigitalOut led2 (D10);
GerhardBerman 0:43160ef59f9f 13 AnalogIn potMeter1(A0);
GerhardBerman 0:43160ef59f9f 14 AnalogIn potMeter2(A1);
GerhardBerman 0:43160ef59f9f 15 DigitalOut motor1DirectionPin(D7);
GerhardBerman 0:43160ef59f9f 16 PwmOut motor1MagnitudePin(D6);
GerhardBerman 0:43160ef59f9f 17 DigitalIn button1(D5);
GerhardBerman 0:43160ef59f9f 18
GerhardBerman 0:43160ef59f9f 19 //set settings
GerhardBerman 0:43160ef59f9f 20 Serial pc(USBTX,USBRX);
GerhardBerman 0:43160ef59f9f 21 Ticker MeasureTicker, BiQuadTicker, FeedbackTickere;// sampleT, TimeTracker;
GerhardBerman 0:43160ef59f9f 22 HIDScope scope(2);
GerhardBerman 0:43160ef59f9f 23
GerhardBerman 0:43160ef59f9f 24 //set datatypes
GerhardBerman 0:43160ef59f9f 25 int counts;
GerhardBerman 0:43160ef59f9f 26 double DerivativeCounts;
GerhardBerman 0:43160ef59f9f 27 int countsPrev = 0;
GerhardBerman 0:43160ef59f9f 28 float referenceVelocity = 0;
GerhardBerman 0:43160ef59f9f 29 double bqcDerivativeCounts = 0;
GerhardBerman 0:43160ef59f9f 30 const double PI = 3.141592653589793;
GerhardBerman 0:43160ef59f9f 31 double Potmeter1 = potMeter1.read();
GerhardBerman 0:43160ef59f9f 32 double Potmeter2 = potMeter2.read();
GerhardBerman 0:43160ef59f9f 33 const int cw = 1;
GerhardBerman 0:43160ef59f9f 34 const int ccw = 0;
GerhardBerman 0:43160ef59f9f 35
GerhardBerman 0:43160ef59f9f 36 //define encoder counts and degrees
GerhardBerman 0:43160ef59f9f 37 QEI Encoder(D12, D13, NC, 32); // turns on encoder
GerhardBerman 0:43160ef59f9f 38 int counts_per_revolution = 4200;
GerhardBerman 0:43160ef59f9f 39 const double gear_ratio = 131;
GerhardBerman 0:43160ef59f9f 40 const double resolution = counts_per_revolution/(2*PI/gear_ratio); //counts per radian
GerhardBerman 0:43160ef59f9f 41
GerhardBerman 0:43160ef59f9f 42 //set BiQuad
GerhardBerman 0:43160ef59f9f 43 BiQuadChain bqc;
GerhardBerman 0:43160ef59f9f 44 BiQuad bq1(0.0186, 0.0743, 0.1114, 0.0743, 0.0186); //get numbers from butter filter MATLAB
GerhardBerman 0:43160ef59f9f 45 BiQuad bq2(1.0000, -1.5704, 1.2756, -0.4844, 0.0762);
GerhardBerman 0:43160ef59f9f 46
GerhardBerman 0:43160ef59f9f 47 //set go-Ticker settings
GerhardBerman 0:43160ef59f9f 48 volatile bool MeasurePTicker_go=false, PTicker_go=false, MotorControllerTicker_go=false;// TimeTracker_go=false, sampleT_go=false;
GerhardBerman 0:43160ef59f9f 49 void MeasurePTicker_act(){MeasurePTicker_go=true;}; // Activates go-flags
GerhardBerman 0:43160ef59f9f 50 void PTicker_act(){PTicker_go=true;};
GerhardBerman 0:43160ef59f9f 51 void MotorControllerTicker_act(){MotorControllerTicker_go=true;};
GerhardBerman 0:43160ef59f9f 52 //void TimeTracker_act(){TimeTracker_go=true;};
GerhardBerman 0:43160ef59f9f 53 //void sampleT_act(){sampleT_go=true;};
GerhardBerman 0:43160ef59f9f 54
GerhardBerman 0:43160ef59f9f 55 /*
GerhardBerman 0:43160ef59f9f 56 float GetReferenceVelocity()
GerhardBerman 0:43160ef59f9f 57 {
GerhardBerman 0:43160ef59f9f 58 // Returns reference velocity in rad/s.
GerhardBerman 0:43160ef59f9f 59 // Positive value means clockwise rotation.
GerhardBerman 0:43160ef59f9f 60 const float maxVelocity = 8.4; // in rad/s of course!
GerhardBerman 0:43160ef59f9f 61 if (button1 == 0){
GerhardBerman 0:43160ef59f9f 62 led1=1;
GerhardBerman 0:43160ef59f9f 63 led2=0;
GerhardBerman 0:43160ef59f9f 64 // Counterclockwise rotation
GerhardBerman 0:43160ef59f9f 65 referenceVelocity = potMeterIn * maxVelocity;
GerhardBerman 0:43160ef59f9f 66 }
GerhardBerman 0:43160ef59f9f 67 else {
GerhardBerman 0:43160ef59f9f 68 led1=0;
GerhardBerman 0:43160ef59f9f 69 led2=1;
GerhardBerman 0:43160ef59f9f 70 // Clockwise rotation
GerhardBerman 0:43160ef59f9f 71 referenceVelocity = -1*potMeterIn * maxVelocity;
GerhardBerman 0:43160ef59f9f 72 }
GerhardBerman 0:43160ef59f9f 73 return referenceVelocity;
GerhardBerman 0:43160ef59f9f 74 }
GerhardBerman 0:43160ef59f9f 75
GerhardBerman 0:43160ef59f9f 76 float FeedForwardControl(float referenceVelocity)
GerhardBerman 0:43160ef59f9f 77 {
GerhardBerman 0:43160ef59f9f 78 // very simple linear feed-forward control
GerhardBerman 0:43160ef59f9f 79 const float MotorGain=8.4; // unit: (rad/s) / PWM
GerhardBerman 0:43160ef59f9f 80 float motorValue = referenceVelocity / MotorGain;
GerhardBerman 0:43160ef59f9f 81 return motorValue;
GerhardBerman 0:43160ef59f9f 82 }
GerhardBerman 0:43160ef59f9f 83
GerhardBerman 0:43160ef59f9f 84 void SetMotor1(float motorValue)
GerhardBerman 0:43160ef59f9f 85 {
GerhardBerman 0:43160ef59f9f 86 // Given -1<=motorValue<=1, this sets the PWM and direction
GerhardBerman 0:43160ef59f9f 87 // bits for motor 1. Positive value makes motor rotating
GerhardBerman 0:43160ef59f9f 88 // clockwise. motorValues outside range are truncated to
GerhardBerman 0:43160ef59f9f 89 // within range
GerhardBerman 0:43160ef59f9f 90 if (motorValue >=0) motor1DirectionPin=1;
GerhardBerman 0:43160ef59f9f 91 else motor1DirectionPin=0;
GerhardBerman 0:43160ef59f9f 92 if (fabs(motorValue)>1) motor1MagnitudePin = 1;
GerhardBerman 0:43160ef59f9f 93 else motor1MagnitudePin = fabs(motorValue);
GerhardBerman 0:43160ef59f9f 94 }
GerhardBerman 0:43160ef59f9f 95
GerhardBerman 0:43160ef59f9f 96 void MeasureAndControl()
GerhardBerman 0:43160ef59f9f 97 {
GerhardBerman 0:43160ef59f9f 98 // This function measures the potmeter position, extracts a
GerhardBerman 0:43160ef59f9f 99 // reference velocity from it, and controls the motor with
GerhardBerman 0:43160ef59f9f 100 // a simple FeedForward controller. Call this from a Ticker.
GerhardBerman 0:43160ef59f9f 101 float referenceVelocity = GetReferenceVelocity();
GerhardBerman 0:43160ef59f9f 102 float motorValue = FeedForwardControl(referenceVelocity);
GerhardBerman 0:43160ef59f9f 103 SetMotor1(motorValue);
GerhardBerman 0:43160ef59f9f 104 }
GerhardBerman 0:43160ef59f9f 105
GerhardBerman 0:43160ef59f9f 106 void TimeTrackerF(){
GerhardBerman 0:43160ef59f9f 107 wait(1);
GerhardBerman 0:43160ef59f9f 108 float Potmeter = potMeterIn.read();
GerhardBerman 0:43160ef59f9f 109 pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
GerhardBerman 0:43160ef59f9f 110 pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
GerhardBerman 0:43160ef59f9f 111 //pc.printf("Counts: %i rad/s \r\n", counts);
GerhardBerman 0:43160ef59f9f 112 //pc.printf("Derivative Counts: %i rad/s \r\n", DerivativeCounts);
GerhardBerman 0:43160ef59f9f 113 }
GerhardBerman 0:43160ef59f9f 114
GerhardBerman 0:43160ef59f9f 115 void sample()
GerhardBerman 0:43160ef59f9f 116 {
GerhardBerman 0:43160ef59f9f 117 int countsPrev = 0;
GerhardBerman 0:43160ef59f9f 118 QEI Encoder(D12, D13, NC, 32);
GerhardBerman 0:43160ef59f9f 119 counts = Encoder.getPulses(); // gives position
GerhardBerman 0:43160ef59f9f 120 //scope.set(0,counts);
GerhardBerman 0:43160ef59f9f 121 DerivativeCounts = (counts-countsPrev)/0.001;
GerhardBerman 0:43160ef59f9f 122 //scope.set(1,DerivativeCounts);
GerhardBerman 0:43160ef59f9f 123 countsPrev = counts;
GerhardBerman 0:43160ef59f9f 124 //scope.send();
GerhardBerman 0:43160ef59f9f 125 pc.printf("Counts: %i rad/s \r\n", counts);
GerhardBerman 0:43160ef59f9f 126 pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts);
GerhardBerman 0:43160ef59f9f 127 }
GerhardBerman 0:43160ef59f9f 128
GerhardBerman 0:43160ef59f9f 129 void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts
GerhardBerman 0:43160ef59f9f 130 //double in=DerivativeCounts();
GerhardBerman 0:43160ef59f9f 131 bqcDerivativeCounts=bqc.step(DerivativeCounts);
GerhardBerman 0:43160ef59f9f 132 //return(bqcDerivativeCounts);
GerhardBerman 0:43160ef59f9f 133 }
GerhardBerman 0:43160ef59f9f 134 */
GerhardBerman 0:43160ef59f9f 135 void MeasureP(){
GerhardBerman 0:43160ef59f9f 136 double ref_position = Potmeter1; //reference position from potmeter
GerhardBerman 0:43160ef59f9f 137 int counts = Encoder.getPulses(); // gives position
GerhardBerman 0:43160ef59f9f 138 double position = counts/resolution; //position in radians
GerhardBerman 0:43160ef59f9f 139 double rotation = ref_position-position; //rotation is 'position error' in radians
GerhardBerman 0:43160ef59f9f 140 double movement = rotation/(2*PI); //movement in rotations
GerhardBerman 0:43160ef59f9f 141 double Kp = Potmeter2;
GerhardBerman 0:43160ef59f9f 142 }
GerhardBerman 0:43160ef59f9f 143
GerhardBerman 0:43160ef59f9f 144 double P(double rotation, double Kp){
GerhardBerman 0:43160ef59f9f 145 double P_output = Kp*movement;
GerhardBerman 0:43160ef59f9f 146 return P_output;
GerhardBerman 0:43160ef59f9f 147 }
GerhardBerman 0:43160ef59f9f 148
GerhardBerman 0:43160ef59f9f 149 void MotorController(){
GerhardBerman 0:43160ef59f9f 150 double output = P(rotation, Kp);
GerhardBerman 0:43160ef59f9f 151
GerhardBerman 0:43160ef59f9f 152 if(rotation>0){
GerhardBerman 0:43160ef59f9f 153 motor1DirectionPin.write(cw);
GerhardBerman 0:43160ef59f9f 154 motor1MagnitudePin.write(output);
GerhardBerman 0:43160ef59f9f 155 }
GerhardBerman 0:43160ef59f9f 156 if(rotation<0){
GerhardBerman 0:43160ef59f9f 157 motor1DirectionPin.write(ccw);
GerhardBerman 0:43160ef59f9f 158 motor1MagnitudePin.write(-output);
GerhardBerman 0:43160ef59f9f 159 }
GerhardBerman 0:43160ef59f9f 160 }
GerhardBerman 0:43160ef59f9f 161
GerhardBerman 0:43160ef59f9f 162 int main()
GerhardBerman 0:43160ef59f9f 163 {
GerhardBerman 0:43160ef59f9f 164 //Initialize
GerhardBerman 0:43160ef59f9f 165 led1=0;
GerhardBerman 0:43160ef59f9f 166 led2=0;
GerhardBerman 0:43160ef59f9f 167 float Potmeter = potMeterIn.read();
GerhardBerman 0:43160ef59f9f 168 MeasureP.attach(&MeasureP_act, 0.01f);
GerhardBerman 0:43160ef59f9f 169 P.attach(&P_act, 0.01f);
GerhardBerman 0:43160ef59f9f 170 MotorController.attach(&MotorController_act, 0.01f);
GerhardBerman 0:43160ef59f9f 171 pc.baud(115200);
GerhardBerman 0:43160ef59f9f 172 QEI Encoder(D12, D13, NC, 32); // turns on encoder
GerhardBerman 0:43160ef59f9f 173 //sampleT.attach(&sampleT_act, 0.1f);
GerhardBerman 0:43160ef59f9f 174 //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
GerhardBerman 0:43160ef59f9f 175 //pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
GerhardBerman 0:43160ef59f9f 176
GerhardBerman 0:43160ef59f9f 177 while(1)
GerhardBerman 0:43160ef59f9f 178 {
GerhardBerman 0:43160ef59f9f 179 if (MeasureP_go){
GerhardBerman 0:43160ef59f9f 180 MeasureP_go=false;
GerhardBerman 0:43160ef59f9f 181 MeasureP();
GerhardBerman 0:43160ef59f9f 182 // Encoder part
GerhardBerman 0:43160ef59f9f 183 /*
GerhardBerman 0:43160ef59f9f 184 counts = Encoder.getPulses(); // gives position
GerhardBerman 0:43160ef59f9f 185 DerivativeCounts = ((double) counts-countsPrev)/0.01;
GerhardBerman 0:43160ef59f9f 186
GerhardBerman 0:43160ef59f9f 187 scope.set(0,counts);
GerhardBerman 0:43160ef59f9f 188 scope.set(1,DerivativeCounts);
GerhardBerman 0:43160ef59f9f 189 //scope.set(1,bqcDerivativeCounts);
GerhardBerman 0:43160ef59f9f 190 scope.send();
GerhardBerman 0:43160ef59f9f 191 countsPrev = counts;
GerhardBerman 0:43160ef59f9f 192 //pc.printf("Counts: %i rad/s \r\n", counts);
GerhardBerman 0:43160ef59f9f 193 //pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts);
GerhardBerman 0:43160ef59f9f 194 */
GerhardBerman 0:43160ef59f9f 195 }
GerhardBerman 0:43160ef59f9f 196
GerhardBerman 0:43160ef59f9f 197 if (PTicker_go){
GerhardBerman 0:43160ef59f9f 198 PTicker_go=false;
GerhardBerman 0:43160ef59f9f 199 P();
GerhardBerman 0:43160ef59f9f 200 }
GerhardBerman 0:43160ef59f9f 201 if (MotorControllerTicker_go){
GerhardBerman 0:43160ef59f9f 202 MotorControllerTicker_go=false;
GerhardBerman 0:43160ef59f9f 203 MotorController();
GerhardBerman 0:43160ef59f9f 204 /*if (TimeTracker_go){
GerhardBerman 0:43160ef59f9f 205 TimeTracker_go=false;
GerhardBerman 0:43160ef59f9f 206 TimeTrackerF();
GerhardBerman 0:43160ef59f9f 207 }
GerhardBerman 0:43160ef59f9f 208 if (sampleT_go){
GerhardBerman 0:43160ef59f9f 209 sampleT_go=false;
GerhardBerman 0:43160ef59f9f 210 sample();
GerhardBerman 0:43160ef59f9f 211 }*/
GerhardBerman 0:43160ef59f9f 212 }
GerhardBerman 0:43160ef59f9f 213 }