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@5:37e230689418, 2016-10-17 (annotated)

Committer:
GerhardBerman
Date:
Mon Oct 17 13:10:20 2016 +0000
Revision:
5:37e230689418
Parent:
4:19e376d31380
Child:
6:3c4f3f2ce54f
PI Controller working with HIDScope

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 3:8caef4872b0c 13 AnalogIn potMeter1(A2);
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 3:8caef4872b0c 21 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
GerhardBerman 5:37e230689418 22 HIDScope scope(2);
GerhardBerman 0:43160ef59f9f 23
GerhardBerman 0:43160ef59f9f 24 //set datatypes
GerhardBerman 3:8caef4872b0c 25 int counts = 0;
GerhardBerman 0:43160ef59f9f 26 double DerivativeCounts;
GerhardBerman 4:19e376d31380 27
GerhardBerman 4:19e376d31380 28 float error_prev = 0;
GerhardBerman 4:19e376d31380 29 float IntError = 0;
GerhardBerman 4:19e376d31380 30 float t_sample = 0.01; //seconds
GerhardBerman 4:19e376d31380 31
GerhardBerman 0:43160ef59f9f 32 int countsPrev = 0;
GerhardBerman 0:43160ef59f9f 33 float referenceVelocity = 0;
GerhardBerman 3:8caef4872b0c 34 float bqcDerivativeCounts = 0;
GerhardBerman 3:8caef4872b0c 35 const float PI = 3.141592653589793;
GerhardBerman 3:8caef4872b0c 36 //float Potmeter1 = potMeter1.read();
GerhardBerman 3:8caef4872b0c 37 //float Potmeter2 = potMeter2.read();
GerhardBerman 3:8caef4872b0c 38 const int cw = 0; //values for cw and ccw are inverted!! cw=0 and ccw=1
GerhardBerman 3:8caef4872b0c 39 const int ccw = 1;
GerhardBerman 0:43160ef59f9f 40
GerhardBerman 0:43160ef59f9f 41 //set BiQuad
GerhardBerman 0:43160ef59f9f 42 BiQuadChain bqc;
GerhardBerman 0:43160ef59f9f 43 BiQuad bq1(0.0186, 0.0743, 0.1114, 0.0743, 0.0186); //get numbers from butter filter MATLAB
GerhardBerman 0:43160ef59f9f 44 BiQuad bq2(1.0000, -1.5704, 1.2756, -0.4844, 0.0762);
GerhardBerman 0:43160ef59f9f 45
GerhardBerman 0:43160ef59f9f 46 //set go-Ticker settings
GerhardBerman 3:8caef4872b0c 47 volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
GerhardBerman 3:8caef4872b0c 48 void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
GerhardBerman 3:8caef4872b0c 49 void BiQuadTicker_act(){BiQuadTicker_go=true;};
GerhardBerman 3:8caef4872b0c 50 void FeedbackTicker_act(){FeedbackTicker_go=true;};
GerhardBerman 3:8caef4872b0c 51 void TimeTracker_act(){TimeTracker_go=true;};
GerhardBerman 3:8caef4872b0c 52 //void sampleT_act(){sampleT_go=true;};
GerhardBerman 3:8caef4872b0c 53
GerhardBerman 3:8caef4872b0c 54 //define encoder counts and degrees
GerhardBerman 3:8caef4872b0c 55 QEI Encoder(D12, D13, NC, 32); // turns on encoder
GerhardBerman 4:19e376d31380 56 const int counts_per_revolution = 4200; //counts per motor axis revolution
GerhardBerman 4:19e376d31380 57 const int inverse_gear_ratio = 131;
GerhardBerman 4:19e376d31380 58 //const float motor_axial_resolution = counts_per_revolution/(2*PI);
GerhardBerman 4:19e376d31380 59 const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio); //87567.0496892 counts per radian, encoder axis
GerhardBerman 3:8caef4872b0c 60
GerhardBerman 3:8caef4872b0c 61 float GetReferencePosition()
GerhardBerman 3:8caef4872b0c 62 {
GerhardBerman 3:8caef4872b0c 63 // Returns reference position in rad.
GerhardBerman 3:8caef4872b0c 64 // Positive value means clockwise rotation.
GerhardBerman 3:8caef4872b0c 65 const float maxPosition = 2*PI; //6.283185307179586; // in radians
GerhardBerman 3:8caef4872b0c 66 float Potmeter1 = potMeter1.read();
GerhardBerman 3:8caef4872b0c 67 float referencePosition = Potmeter1 * maxPosition; //Potmeter1 * maxPosition; //refpos in radians
GerhardBerman 3:8caef4872b0c 68 pc.printf("Max Position: %f rad \r\n", maxPosition);
GerhardBerman 4:19e376d31380 69 pc.printf("Potmeter1, refpos: %f \r\n", Potmeter1);
GerhardBerman 4:19e376d31380 70 pc.printf("Motor Axis Ref Position: %f rad \r\n", referencePosition);
GerhardBerman 3:8caef4872b0c 71 return referencePosition;
GerhardBerman 3:8caef4872b0c 72 }
GerhardBerman 3:8caef4872b0c 73
GerhardBerman 3:8caef4872b0c 74 float FeedForwardControl(float referencePosition)
GerhardBerman 3:8caef4872b0c 75 {
GerhardBerman 4:19e376d31380 76 float EncoderPosition = counts/resolution; //position in radians, encoder axis
GerhardBerman 4:19e376d31380 77 float Position = EncoderPosition*inverse_gear_ratio; //position in radians, motor axis
GerhardBerman 3:8caef4872b0c 78 // linear feedback control
GerhardBerman 5:37e230689418 79 scope.set(0,referencePosition);
GerhardBerman 5:37e230689418 80 scope.set(1,Position);
GerhardBerman 5:37e230689418 81 scope.send();
GerhardBerman 4:19e376d31380 82 float error = referencePosition - Position; // 'error' in radians
GerhardBerman 4:19e376d31380 83 float Kp = 1; //potMeter2.read();
GerhardBerman 4:19e376d31380 84
GerhardBerman 4:19e376d31380 85 float IntError = IntError + error*t_sample;
GerhardBerman 5:37e230689418 86 float maxKi = 0.2;
GerhardBerman 5:37e230689418 87 float Ki = potMeter2.read()*maxKi;
GerhardBerman 4:19e376d31380 88
GerhardBerman 4:19e376d31380 89 //float DerivativeError = (error_prev + error)/t_sample;
GerhardBerman 5:37e230689418 90 float maxKd = 0.2;
GerhardBerman 5:37e230689418 91 //float Kd = potMeter2.read()*maxKd;
GerhardBerman 4:19e376d31380 92
GerhardBerman 4:19e376d31380 93 float motorValue = error * Kp + IntError * Ki; //+ DerivativeError * Kd;
GerhardBerman 4:19e376d31380 94 pc.printf("Motor Axis Position: %f rad \r\n", Position);
GerhardBerman 4:19e376d31380 95 pc.printf("Counts encoder: %i rad \r\n", counts);
GerhardBerman 3:8caef4872b0c 96 pc.printf("Kp: %f \r\n", Kp);
GerhardBerman 3:8caef4872b0c 97 pc.printf("MotorValue: %f \r\n", motorValue);
GerhardBerman 4:19e376d31380 98
GerhardBerman 4:19e376d31380 99 error_prev = error;
GerhardBerman 3:8caef4872b0c 100 return motorValue;
GerhardBerman 3:8caef4872b0c 101 }
GerhardBerman 0:43160ef59f9f 102
GerhardBerman 3:8caef4872b0c 103 void SetMotor1(float motorValue)
GerhardBerman 3:8caef4872b0c 104 {
GerhardBerman 3:8caef4872b0c 105 // Given -1<=motorValue<=1, this sets the PWM and direction
GerhardBerman 3:8caef4872b0c 106 // bits for motor 1. Positive value makes motor rotating
GerhardBerman 3:8caef4872b0c 107 // clockwise. motorValues outside range are truncated to
GerhardBerman 3:8caef4872b0c 108 // within range
GerhardBerman 3:8caef4872b0c 109 if (motorValue >=0)
GerhardBerman 3:8caef4872b0c 110 {motor1DirectionPin=cw;
GerhardBerman 3:8caef4872b0c 111 led1=1;
GerhardBerman 3:8caef4872b0c 112 led2=0;
GerhardBerman 3:8caef4872b0c 113 }
GerhardBerman 3:8caef4872b0c 114 else {motor1DirectionPin=ccw;
GerhardBerman 3:8caef4872b0c 115 led1=0;
GerhardBerman 3:8caef4872b0c 116 led2=1;
GerhardBerman 3:8caef4872b0c 117 }
GerhardBerman 3:8caef4872b0c 118 if (fabs(motorValue)>1) motor1MagnitudePin = 1;
GerhardBerman 3:8caef4872b0c 119 else motor1MagnitudePin = fabs(motorValue);
GerhardBerman 3:8caef4872b0c 120 }
GerhardBerman 3:8caef4872b0c 121
GerhardBerman 3:8caef4872b0c 122 void MeasureAndControl()
GerhardBerman 3:8caef4872b0c 123 {
GerhardBerman 3:8caef4872b0c 124 // This function measures the potmeter position, extracts a
GerhardBerman 3:8caef4872b0c 125 // reference position from it, and controls the motor with
GerhardBerman 3:8caef4872b0c 126 // a Feedback controller. Call this from a Ticker.
GerhardBerman 3:8caef4872b0c 127 float referencePosition = GetReferencePosition();
GerhardBerman 3:8caef4872b0c 128 float motorValue = FeedForwardControl(referencePosition);
GerhardBerman 3:8caef4872b0c 129 SetMotor1(motorValue);
GerhardBerman 3:8caef4872b0c 130 }
GerhardBerman 3:8caef4872b0c 131
GerhardBerman 3:8caef4872b0c 132 void TimeTrackerF(){
GerhardBerman 3:8caef4872b0c 133 //wait(1);
GerhardBerman 3:8caef4872b0c 134 //float Potmeter1 = potMeter1.read();
GerhardBerman 3:8caef4872b0c 135 float referencePosition = GetReferencePosition();
GerhardBerman 3:8caef4872b0c 136 pc.printf("TTReference Position: %d rad \r\n", referencePosition);
GerhardBerman 3:8caef4872b0c 137 //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
GerhardBerman 3:8caef4872b0c 138 //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
GerhardBerman 3:8caef4872b0c 139 pc.printf("TTCounts: %i \r\n", counts);
GerhardBerman 3:8caef4872b0c 140 }
GerhardBerman 3:8caef4872b0c 141 /*
GerhardBerman 3:8caef4872b0c 142 void sample()
GerhardBerman 3:8caef4872b0c 143 {
GerhardBerman 3:8caef4872b0c 144 int countsPrev = 0;
GerhardBerman 3:8caef4872b0c 145 QEI Encoder(D12, D13, NC, 32);
GerhardBerman 3:8caef4872b0c 146 counts = Encoder.getPulses(); // gives position
GerhardBerman 3:8caef4872b0c 147 //scope.set(0,counts);
GerhardBerman 3:8caef4872b0c 148 DerivativeCounts = (counts-countsPrev)/0.001;
GerhardBerman 3:8caef4872b0c 149 //scope.set(1,DerivativeCounts);
GerhardBerman 3:8caef4872b0c 150 countsPrev = counts;
GerhardBerman 3:8caef4872b0c 151 //scope.send();
GerhardBerman 3:8caef4872b0c 152 pc.printf("Counts: %i rad/s \r\n", counts);
GerhardBerman 3:8caef4872b0c 153 pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts);
GerhardBerman 3:8caef4872b0c 154 }
GerhardBerman 3:8caef4872b0c 155
GerhardBerman 3:8caef4872b0c 156 void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts
GerhardBerman 3:8caef4872b0c 157 //double in=DerivativeCounts();
GerhardBerman 3:8caef4872b0c 158 bqcDerivativeCounts=bqc.step(DerivativeCounts);
GerhardBerman 3:8caef4872b0c 159 //return(bqcDerivativeCounts);
GerhardBerman 3:8caef4872b0c 160 }
GerhardBerman 3:8caef4872b0c 161
GerhardBerman 0:43160ef59f9f 162 void MeasureP(){
GerhardBerman 0:43160ef59f9f 163 double ref_position = Potmeter1; //reference position from potmeter
GerhardBerman 0:43160ef59f9f 164 int counts = Encoder.getPulses(); // gives position
GerhardBerman 0:43160ef59f9f 165 double position = counts/resolution; //position in radians
GerhardBerman 0:43160ef59f9f 166 double rotation = ref_position-position; //rotation is 'position error' in radians
GerhardBerman 0:43160ef59f9f 167 double movement = rotation/(2*PI); //movement in rotations
GerhardBerman 0:43160ef59f9f 168 double Kp = Potmeter2;
GerhardBerman 3:8caef4872b0c 169 }
GerhardBerman 3:8caef4872b0c 170
GerhardBerman 3:8caef4872b0c 171 double P(double rotation, double Kp){
GerhardBerman 0:43160ef59f9f 172 double P_output = Kp*movement;
GerhardBerman 3:8caef4872b0c 173 return P_output;
GerhardBerman 3:8caef4872b0c 174 }
GerhardBerman 3:8caef4872b0c 175
GerhardBerman 3:8caef4872b0c 176 void MotorController(){
GerhardBerman 3:8caef4872b0c 177 double output = P(rotation, Kp);
GerhardBerman 0:43160ef59f9f 178
GerhardBerman 0:43160ef59f9f 179 if(rotation>0){
GerhardBerman 0:43160ef59f9f 180 motor1DirectionPin.write(cw);
GerhardBerman 3:8caef4872b0c 181 motor1MagnitudePin.write(output);
GerhardBerman 0:43160ef59f9f 182 }
GerhardBerman 0:43160ef59f9f 183 if(rotation<0){
GerhardBerman 0:43160ef59f9f 184 motor1DirectionPin.write(ccw);
GerhardBerman 3:8caef4872b0c 185 motor1MagnitudePin.write(-output);
GerhardBerman 0:43160ef59f9f 186 }
GerhardBerman 0:43160ef59f9f 187 }
GerhardBerman 3:8caef4872b0c 188 */
GerhardBerman 0:43160ef59f9f 189 int main()
GerhardBerman 0:43160ef59f9f 190 {
GerhardBerman 0:43160ef59f9f 191 //Initialize
GerhardBerman 3:8caef4872b0c 192 led1=1;
GerhardBerman 3:8caef4872b0c 193 led2=1;
GerhardBerman 3:8caef4872b0c 194 pc.baud(115200);
GerhardBerman 3:8caef4872b0c 195 pc.printf("Test putty");
GerhardBerman 3:8caef4872b0c 196 //float Potmeter = potMeterIn.read();
GerhardBerman 4:19e376d31380 197 MeasureTicker.attach(&MeasureTicker_act, 0.01f);
GerhardBerman 3:8caef4872b0c 198 bqc.add(&bq1).add(&bq2);
GerhardBerman 3:8caef4872b0c 199 //BiQuadTicker.attach(&BiQuadTicker_act, 0.01f); //frequentie van 100 Hz
GerhardBerman 3:8caef4872b0c 200 //TimeTracker.attach(&TimeTracker_act, 1.0f);
GerhardBerman 0:43160ef59f9f 201 QEI Encoder(D12, D13, NC, 32); // turns on encoder
GerhardBerman 3:8caef4872b0c 202 //sampleT.attach(&sampleT_act, 0.1f);
GerhardBerman 3:8caef4872b0c 203 //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
GerhardBerman 3:8caef4872b0c 204 //pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
GerhardBerman 0:43160ef59f9f 205
GerhardBerman 0:43160ef59f9f 206 while(1)
GerhardBerman 0:43160ef59f9f 207 {
GerhardBerman 3:8caef4872b0c 208 if (MeasureTicker_go){
GerhardBerman 3:8caef4872b0c 209 MeasureTicker_go=false;
GerhardBerman 3:8caef4872b0c 210 MeasureAndControl();
GerhardBerman 3:8caef4872b0c 211 counts = Encoder.getPulses(); // gives position of encoder
GerhardBerman 3:8caef4872b0c 212 pc.printf("Resolution: %f pulses/rad \r\n",resolution);
GerhardBerman 3:8caef4872b0c 213 }
GerhardBerman 3:8caef4872b0c 214 /*
GerhardBerman 3:8caef4872b0c 215 // Encoder part
GerhardBerman 3:8caef4872b0c 216 counts = Encoder.getPulses(); // gives position
GerhardBerman 3:8caef4872b0c 217 DerivativeCounts = ((double) counts-countsPrev)/0.01;
GerhardBerman 3:8caef4872b0c 218
GerhardBerman 3:8caef4872b0c 219 scope.set(0,counts);
GerhardBerman 3:8caef4872b0c 220 scope.set(1,DerivativeCounts);
GerhardBerman 3:8caef4872b0c 221 //scope.set(1,bqcDerivativeCounts);
GerhardBerman 3:8caef4872b0c 222 scope.send();
GerhardBerman 3:8caef4872b0c 223 countsPrev = counts;
GerhardBerman 3:8caef4872b0c 224 //pc.printf("Counts: %i rad/s \r\n", counts);
GerhardBerman 3:8caef4872b0c 225 //pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts);
GerhardBerman 1:ace33633653b 226 }
GerhardBerman 3:8caef4872b0c 227
GerhardBerman 3:8caef4872b0c 228 if (BiQuadTicker_go){
GerhardBerman 3:8caef4872b0c 229 BiQuadTicker_go=false;
GerhardBerman 3:8caef4872b0c 230 BiQuadFilter();
GerhardBerman 3:8caef4872b0c 231 }
GerhardBerman 3:8caef4872b0c 232
GerhardBerman 3:8caef4872b0c 233 if (FeedbackTicker_go){
GerhardBerman 3:8caef4872b0c 234 FeedbackTicker_go=false;
GerhardBerman 3:8caef4872b0c 235 Feedback();
GerhardBerman 3:8caef4872b0c 236
GerhardBerman 3:8caef4872b0c 237 if (TimeTracker_go){
GerhardBerman 3:8caef4872b0c 238 TimeTracker_go=false;
GerhardBerman 3:8caef4872b0c 239 TimeTrackerF();
GerhardBerman 3:8caef4872b0c 240 }
GerhardBerman 3:8caef4872b0c 241
GerhardBerman 3:8caef4872b0c 242 if (sampleT_go){
GerhardBerman 3:8caef4872b0c 243 sampleT_go=false;
GerhardBerman 3:8caef4872b0c 244 sample();
GerhardBerman 3:8caef4872b0c 245 }*/
GerhardBerman 0:43160ef59f9f 246 }
GerhardBerman 0:43160ef59f9f 247 }