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@4:19e376d31380, 2016-10-17 (annotated)

Committer:
GerhardBerman
Date:
Mon Oct 17 12:52:16 2016 +0000
Revision:
4:19e376d31380
Parent:
3:8caef4872b0c
Child:
5:37e230689418
PI-Controller working!

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 3:8caef4872b0c 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 4:19e376d31380 79 float error = referencePosition - Position; // 'error' in radians
GerhardBerman 4:19e376d31380 80 float Kp = 1; //potMeter2.read();
GerhardBerman 4:19e376d31380 81
GerhardBerman 4:19e376d31380 82 float IntError = IntError + error*t_sample;
GerhardBerman 4:19e376d31380 83 float Ki = potMeter2.read();
GerhardBerman 4:19e376d31380 84
GerhardBerman 4:19e376d31380 85 //float DerivativeError = (error_prev + error)/t_sample;
GerhardBerman 4:19e376d31380 86 //float Kd = 1;
GerhardBerman 4:19e376d31380 87
GerhardBerman 4:19e376d31380 88 float motorValue = error * Kp + IntError * Ki; //+ DerivativeError * Kd;
GerhardBerman 4:19e376d31380 89 pc.printf("Motor Axis Position: %f rad \r\n", Position);
GerhardBerman 4:19e376d31380 90 pc.printf("Counts encoder: %i rad \r\n", counts);
GerhardBerman 3:8caef4872b0c 91 pc.printf("Kp: %f \r\n", Kp);
GerhardBerman 3:8caef4872b0c 92 pc.printf("MotorValue: %f \r\n", motorValue);
GerhardBerman 4:19e376d31380 93
GerhardBerman 4:19e376d31380 94 error_prev = error;
GerhardBerman 3:8caef4872b0c 95 return motorValue;
GerhardBerman 3:8caef4872b0c 96 }
GerhardBerman 0:43160ef59f9f 97
GerhardBerman 3:8caef4872b0c 98 void SetMotor1(float motorValue)
GerhardBerman 3:8caef4872b0c 99 {
GerhardBerman 3:8caef4872b0c 100 // Given -1<=motorValue<=1, this sets the PWM and direction
GerhardBerman 3:8caef4872b0c 101 // bits for motor 1. Positive value makes motor rotating
GerhardBerman 3:8caef4872b0c 102 // clockwise. motorValues outside range are truncated to
GerhardBerman 3:8caef4872b0c 103 // within range
GerhardBerman 3:8caef4872b0c 104 if (motorValue >=0)
GerhardBerman 3:8caef4872b0c 105 {motor1DirectionPin=cw;
GerhardBerman 3:8caef4872b0c 106 led1=1;
GerhardBerman 3:8caef4872b0c 107 led2=0;
GerhardBerman 3:8caef4872b0c 108 }
GerhardBerman 3:8caef4872b0c 109 else {motor1DirectionPin=ccw;
GerhardBerman 3:8caef4872b0c 110 led1=0;
GerhardBerman 3:8caef4872b0c 111 led2=1;
GerhardBerman 3:8caef4872b0c 112 }
GerhardBerman 3:8caef4872b0c 113 if (fabs(motorValue)>1) motor1MagnitudePin = 1;
GerhardBerman 3:8caef4872b0c 114 else motor1MagnitudePin = fabs(motorValue);
GerhardBerman 3:8caef4872b0c 115 }
GerhardBerman 3:8caef4872b0c 116
GerhardBerman 3:8caef4872b0c 117 void MeasureAndControl()
GerhardBerman 3:8caef4872b0c 118 {
GerhardBerman 3:8caef4872b0c 119 // This function measures the potmeter position, extracts a
GerhardBerman 3:8caef4872b0c 120 // reference position from it, and controls the motor with
GerhardBerman 3:8caef4872b0c 121 // a Feedback controller. Call this from a Ticker.
GerhardBerman 3:8caef4872b0c 122 float referencePosition = GetReferencePosition();
GerhardBerman 3:8caef4872b0c 123 float motorValue = FeedForwardControl(referencePosition);
GerhardBerman 3:8caef4872b0c 124 SetMotor1(motorValue);
GerhardBerman 3:8caef4872b0c 125 }
GerhardBerman 3:8caef4872b0c 126
GerhardBerman 3:8caef4872b0c 127 void TimeTrackerF(){
GerhardBerman 3:8caef4872b0c 128 //wait(1);
GerhardBerman 3:8caef4872b0c 129 //float Potmeter1 = potMeter1.read();
GerhardBerman 3:8caef4872b0c 130 float referencePosition = GetReferencePosition();
GerhardBerman 3:8caef4872b0c 131 pc.printf("TTReference Position: %d rad \r\n", referencePosition);
GerhardBerman 3:8caef4872b0c 132 //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
GerhardBerman 3:8caef4872b0c 133 //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
GerhardBerman 3:8caef4872b0c 134 pc.printf("TTCounts: %i \r\n", counts);
GerhardBerman 3:8caef4872b0c 135 }
GerhardBerman 3:8caef4872b0c 136 /*
GerhardBerman 3:8caef4872b0c 137 void sample()
GerhardBerman 3:8caef4872b0c 138 {
GerhardBerman 3:8caef4872b0c 139 int countsPrev = 0;
GerhardBerman 3:8caef4872b0c 140 QEI Encoder(D12, D13, NC, 32);
GerhardBerman 3:8caef4872b0c 141 counts = Encoder.getPulses(); // gives position
GerhardBerman 3:8caef4872b0c 142 //scope.set(0,counts);
GerhardBerman 3:8caef4872b0c 143 DerivativeCounts = (counts-countsPrev)/0.001;
GerhardBerman 3:8caef4872b0c 144 //scope.set(1,DerivativeCounts);
GerhardBerman 3:8caef4872b0c 145 countsPrev = counts;
GerhardBerman 3:8caef4872b0c 146 //scope.send();
GerhardBerman 3:8caef4872b0c 147 pc.printf("Counts: %i rad/s \r\n", counts);
GerhardBerman 3:8caef4872b0c 148 pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts);
GerhardBerman 3:8caef4872b0c 149 }
GerhardBerman 3:8caef4872b0c 150
GerhardBerman 3:8caef4872b0c 151 void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts
GerhardBerman 3:8caef4872b0c 152 //double in=DerivativeCounts();
GerhardBerman 3:8caef4872b0c 153 bqcDerivativeCounts=bqc.step(DerivativeCounts);
GerhardBerman 3:8caef4872b0c 154 //return(bqcDerivativeCounts);
GerhardBerman 3:8caef4872b0c 155 }
GerhardBerman 3:8caef4872b0c 156
GerhardBerman 0:43160ef59f9f 157 void MeasureP(){
GerhardBerman 0:43160ef59f9f 158 double ref_position = Potmeter1; //reference position from potmeter
GerhardBerman 0:43160ef59f9f 159 int counts = Encoder.getPulses(); // gives position
GerhardBerman 0:43160ef59f9f 160 double position = counts/resolution; //position in radians
GerhardBerman 0:43160ef59f9f 161 double rotation = ref_position-position; //rotation is 'position error' in radians
GerhardBerman 0:43160ef59f9f 162 double movement = rotation/(2*PI); //movement in rotations
GerhardBerman 0:43160ef59f9f 163 double Kp = Potmeter2;
GerhardBerman 3:8caef4872b0c 164 }
GerhardBerman 3:8caef4872b0c 165
GerhardBerman 3:8caef4872b0c 166 double P(double rotation, double Kp){
GerhardBerman 0:43160ef59f9f 167 double P_output = Kp*movement;
GerhardBerman 3:8caef4872b0c 168 return P_output;
GerhardBerman 3:8caef4872b0c 169 }
GerhardBerman 3:8caef4872b0c 170
GerhardBerman 3:8caef4872b0c 171 void MotorController(){
GerhardBerman 3:8caef4872b0c 172 double output = P(rotation, Kp);
GerhardBerman 0:43160ef59f9f 173
GerhardBerman 0:43160ef59f9f 174 if(rotation>0){
GerhardBerman 0:43160ef59f9f 175 motor1DirectionPin.write(cw);
GerhardBerman 3:8caef4872b0c 176 motor1MagnitudePin.write(output);
GerhardBerman 0:43160ef59f9f 177 }
GerhardBerman 0:43160ef59f9f 178 if(rotation<0){
GerhardBerman 0:43160ef59f9f 179 motor1DirectionPin.write(ccw);
GerhardBerman 3:8caef4872b0c 180 motor1MagnitudePin.write(-output);
GerhardBerman 0:43160ef59f9f 181 }
GerhardBerman 0:43160ef59f9f 182 }
GerhardBerman 3:8caef4872b0c 183 */
GerhardBerman 0:43160ef59f9f 184 int main()
GerhardBerman 0:43160ef59f9f 185 {
GerhardBerman 0:43160ef59f9f 186 //Initialize
GerhardBerman 3:8caef4872b0c 187 led1=1;
GerhardBerman 3:8caef4872b0c 188 led2=1;
GerhardBerman 3:8caef4872b0c 189 pc.baud(115200);
GerhardBerman 3:8caef4872b0c 190 pc.printf("Test putty");
GerhardBerman 3:8caef4872b0c 191 //float Potmeter = potMeterIn.read();
GerhardBerman 4:19e376d31380 192 MeasureTicker.attach(&MeasureTicker_act, 0.01f);
GerhardBerman 3:8caef4872b0c 193 bqc.add(&bq1).add(&bq2);
GerhardBerman 3:8caef4872b0c 194 //BiQuadTicker.attach(&BiQuadTicker_act, 0.01f); //frequentie van 100 Hz
GerhardBerman 3:8caef4872b0c 195 //TimeTracker.attach(&TimeTracker_act, 1.0f);
GerhardBerman 0:43160ef59f9f 196 QEI Encoder(D12, D13, NC, 32); // turns on encoder
GerhardBerman 3:8caef4872b0c 197 //sampleT.attach(&sampleT_act, 0.1f);
GerhardBerman 3:8caef4872b0c 198 //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
GerhardBerman 3:8caef4872b0c 199 //pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
GerhardBerman 0:43160ef59f9f 200
GerhardBerman 0:43160ef59f9f 201 while(1)
GerhardBerman 0:43160ef59f9f 202 {
GerhardBerman 3:8caef4872b0c 203 if (MeasureTicker_go){
GerhardBerman 3:8caef4872b0c 204 MeasureTicker_go=false;
GerhardBerman 3:8caef4872b0c 205 MeasureAndControl();
GerhardBerman 3:8caef4872b0c 206 counts = Encoder.getPulses(); // gives position of encoder
GerhardBerman 3:8caef4872b0c 207 pc.printf("Resolution: %f pulses/rad \r\n",resolution);
GerhardBerman 3:8caef4872b0c 208 }
GerhardBerman 3:8caef4872b0c 209 /*
GerhardBerman 3:8caef4872b0c 210 // Encoder part
GerhardBerman 3:8caef4872b0c 211 counts = Encoder.getPulses(); // gives position
GerhardBerman 3:8caef4872b0c 212 DerivativeCounts = ((double) counts-countsPrev)/0.01;
GerhardBerman 3:8caef4872b0c 213
GerhardBerman 3:8caef4872b0c 214 scope.set(0,counts);
GerhardBerman 3:8caef4872b0c 215 scope.set(1,DerivativeCounts);
GerhardBerman 3:8caef4872b0c 216 //scope.set(1,bqcDerivativeCounts);
GerhardBerman 3:8caef4872b0c 217 scope.send();
GerhardBerman 3:8caef4872b0c 218 countsPrev = counts;
GerhardBerman 3:8caef4872b0c 219 //pc.printf("Counts: %i rad/s \r\n", counts);
GerhardBerman 3:8caef4872b0c 220 //pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts);
GerhardBerman 1:ace33633653b 221 }
GerhardBerman 3:8caef4872b0c 222
GerhardBerman 3:8caef4872b0c 223 if (BiQuadTicker_go){
GerhardBerman 3:8caef4872b0c 224 BiQuadTicker_go=false;
GerhardBerman 3:8caef4872b0c 225 BiQuadFilter();
GerhardBerman 3:8caef4872b0c 226 }
GerhardBerman 3:8caef4872b0c 227
GerhardBerman 3:8caef4872b0c 228 if (FeedbackTicker_go){
GerhardBerman 3:8caef4872b0c 229 FeedbackTicker_go=false;
GerhardBerman 3:8caef4872b0c 230 Feedback();
GerhardBerman 3:8caef4872b0c 231
GerhardBerman 3:8caef4872b0c 232 if (TimeTracker_go){
GerhardBerman 3:8caef4872b0c 233 TimeTracker_go=false;
GerhardBerman 3:8caef4872b0c 234 TimeTrackerF();
GerhardBerman 3:8caef4872b0c 235 }
GerhardBerman 3:8caef4872b0c 236
GerhardBerman 3:8caef4872b0c 237 if (sampleT_go){
GerhardBerman 3:8caef4872b0c 238 sampleT_go=false;
GerhardBerman 3:8caef4872b0c 239 sample();
GerhardBerman 3:8caef4872b0c 240 }*/
GerhardBerman 0:43160ef59f9f 241 }
GerhardBerman 0:43160ef59f9f 242 }