Gerhard Berman / Mbed 2 deprecated prog_forwardkin_feedback_copy

Working, but boundaries not yet tested

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_inversekin_feedback by Gerhard Berman

main.cpp@3:8caef4872b0c, 2016-10-17 (annotated)

Committer:
GerhardBerman
Date:
Mon Oct 17 10:39:56 2016 +0000
Revision:
3:8caef4872b0c
Parent:
2:94b5e00288a5
Child:
4:19e376d31380
All working for feedback regulation

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