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@6:3c4f3f2ce54f, 2016-10-17 (annotated)

Committer:
GerhardBerman
Date:
Mon Oct 17 14:34:25 2016 +0000
Revision:
6:3c4f3f2ce54f
Parent:
5:37e230689418
Child:
7:2f74dfd1d411
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 6:3c4f3f2ce54f 22 HIDScope scope(3);
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 6:3c4f3f2ce54f 79
GerhardBerman 6:3c4f3f2ce54f 80 float error = referencePosition - Position; // proportional error in radians
GerhardBerman 4:19e376d31380 81 float Kp = 1; //potMeter2.read();
GerhardBerman 4:19e376d31380 82
GerhardBerman 6:3c4f3f2ce54f 83 float IntError = IntError + error*t_sample; // integrated error in radians
GerhardBerman 5:37e230689418 84 float maxKi = 0.2;
GerhardBerman 6:3c4f3f2ce54f 85 float Ki = potMeter2.read(); //*maxKi;
GerhardBerman 4:19e376d31380 86
GerhardBerman 6:3c4f3f2ce54f 87 /*
GerhardBerman 6:3c4f3f2ce54f 88 float DerivativeError = (error_prev + error)/t_sample; // derivative of error in radians
GerhardBerman 5:37e230689418 89 float maxKd = 0.2;
GerhardBerman 6:3c4f3f2ce54f 90 float Kd = potMeter2.read()*maxKd;
GerhardBerman 6:3c4f3f2ce54f 91 */
GerhardBerman 4:19e376d31380 92
GerhardBerman 6:3c4f3f2ce54f 93 scope.set(0,referencePosition);
GerhardBerman 6:3c4f3f2ce54f 94 scope.set(1,Position);
GerhardBerman 6:3c4f3f2ce54f 95 scope.set(2,Ki);
GerhardBerman 6:3c4f3f2ce54f 96 scope.send();
GerhardBerman 6:3c4f3f2ce54f 97
GerhardBerman 6:3c4f3f2ce54f 98 float motorValue = error * Kp + IntError * Ki; // + DerivativeError * Kd; //total controller output = motor input
GerhardBerman 4:19e376d31380 99 pc.printf("Motor Axis Position: %f rad \r\n", Position);
GerhardBerman 4:19e376d31380 100 pc.printf("Counts encoder: %i rad \r\n", counts);
GerhardBerman 3:8caef4872b0c 101 pc.printf("Kp: %f \r\n", Kp);
GerhardBerman 3:8caef4872b0c 102 pc.printf("MotorValue: %f \r\n", motorValue);
GerhardBerman 4:19e376d31380 103
GerhardBerman 4:19e376d31380 104 error_prev = error;
GerhardBerman 3:8caef4872b0c 105 return motorValue;
GerhardBerman 3:8caef4872b0c 106 }
GerhardBerman 0:43160ef59f9f 107
GerhardBerman 3:8caef4872b0c 108 void SetMotor1(float motorValue)
GerhardBerman 3:8caef4872b0c 109 {
GerhardBerman 3:8caef4872b0c 110 // Given -1<=motorValue<=1, this sets the PWM and direction
GerhardBerman 3:8caef4872b0c 111 // bits for motor 1. Positive value makes motor rotating
GerhardBerman 3:8caef4872b0c 112 // clockwise. motorValues outside range are truncated to
GerhardBerman 3:8caef4872b0c 113 // within range
GerhardBerman 3:8caef4872b0c 114 if (motorValue >=0)
GerhardBerman 3:8caef4872b0c 115 {motor1DirectionPin=cw;
GerhardBerman 3:8caef4872b0c 116 led1=1;
GerhardBerman 3:8caef4872b0c 117 led2=0;
GerhardBerman 3:8caef4872b0c 118 }
GerhardBerman 3:8caef4872b0c 119 else {motor1DirectionPin=ccw;
GerhardBerman 3:8caef4872b0c 120 led1=0;
GerhardBerman 3:8caef4872b0c 121 led2=1;
GerhardBerman 3:8caef4872b0c 122 }
GerhardBerman 3:8caef4872b0c 123 if (fabs(motorValue)>1) motor1MagnitudePin = 1;
GerhardBerman 3:8caef4872b0c 124 else motor1MagnitudePin = fabs(motorValue);
GerhardBerman 3:8caef4872b0c 125 }
GerhardBerman 3:8caef4872b0c 126
GerhardBerman 3:8caef4872b0c 127 void MeasureAndControl()
GerhardBerman 3:8caef4872b0c 128 {
GerhardBerman 3:8caef4872b0c 129 // This function measures the potmeter position, extracts a
GerhardBerman 3:8caef4872b0c 130 // reference position from it, and controls the motor with
GerhardBerman 3:8caef4872b0c 131 // a Feedback controller. Call this from a Ticker.
GerhardBerman 3:8caef4872b0c 132 float referencePosition = GetReferencePosition();
GerhardBerman 3:8caef4872b0c 133 float motorValue = FeedForwardControl(referencePosition);
GerhardBerman 3:8caef4872b0c 134 SetMotor1(motorValue);
GerhardBerman 3:8caef4872b0c 135 }
GerhardBerman 3:8caef4872b0c 136
GerhardBerman 3:8caef4872b0c 137 void TimeTrackerF(){
GerhardBerman 3:8caef4872b0c 138 //wait(1);
GerhardBerman 3:8caef4872b0c 139 //float Potmeter1 = potMeter1.read();
GerhardBerman 3:8caef4872b0c 140 float referencePosition = GetReferencePosition();
GerhardBerman 3:8caef4872b0c 141 pc.printf("TTReference Position: %d rad \r\n", referencePosition);
GerhardBerman 3:8caef4872b0c 142 //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
GerhardBerman 3:8caef4872b0c 143 //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
GerhardBerman 3:8caef4872b0c 144 pc.printf("TTCounts: %i \r\n", counts);
GerhardBerman 3:8caef4872b0c 145 }
GerhardBerman 3:8caef4872b0c 146 /*
GerhardBerman 3:8caef4872b0c 147 void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts
GerhardBerman 3:8caef4872b0c 148 //double in=DerivativeCounts();
GerhardBerman 3:8caef4872b0c 149 bqcDerivativeCounts=bqc.step(DerivativeCounts);
GerhardBerman 3:8caef4872b0c 150 //return(bqcDerivativeCounts);
GerhardBerman 3:8caef4872b0c 151 }
GerhardBerman 6:3c4f3f2ce54f 152 */
GerhardBerman 6:3c4f3f2ce54f 153
GerhardBerman 0:43160ef59f9f 154 int main()
GerhardBerman 0:43160ef59f9f 155 {
GerhardBerman 0:43160ef59f9f 156 //Initialize
GerhardBerman 3:8caef4872b0c 157 led1=1;
GerhardBerman 3:8caef4872b0c 158 led2=1;
GerhardBerman 3:8caef4872b0c 159 pc.baud(115200);
GerhardBerman 3:8caef4872b0c 160 pc.printf("Test putty");
GerhardBerman 4:19e376d31380 161 MeasureTicker.attach(&MeasureTicker_act, 0.01f);
GerhardBerman 3:8caef4872b0c 162 bqc.add(&bq1).add(&bq2);
GerhardBerman 0:43160ef59f9f 163 QEI Encoder(D12, D13, NC, 32); // turns on encoder
GerhardBerman 0:43160ef59f9f 164
GerhardBerman 0:43160ef59f9f 165 while(1)
GerhardBerman 0:43160ef59f9f 166 {
GerhardBerman 3:8caef4872b0c 167 if (MeasureTicker_go){
GerhardBerman 3:8caef4872b0c 168 MeasureTicker_go=false;
GerhardBerman 3:8caef4872b0c 169 MeasureAndControl();
GerhardBerman 3:8caef4872b0c 170 counts = Encoder.getPulses(); // gives position of encoder
GerhardBerman 3:8caef4872b0c 171 pc.printf("Resolution: %f pulses/rad \r\n",resolution);
GerhardBerman 3:8caef4872b0c 172 }
GerhardBerman 6:3c4f3f2ce54f 173 /*
GerhardBerman 3:8caef4872b0c 174 if (BiQuadTicker_go){
GerhardBerman 3:8caef4872b0c 175 BiQuadTicker_go=false;
GerhardBerman 3:8caef4872b0c 176 BiQuadFilter();
GerhardBerman 3:8caef4872b0c 177 }
GerhardBerman 6:3c4f3f2ce54f 178 */
GerhardBerman 0:43160ef59f9f 179 }
GerhardBerman 0:43160ef59f9f 180 }