Gerhard Berman / Mbed 2 deprecated EMG_prog_forwardkin_feedback

EMG added to main IK program. No errors, not yet tested

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_forwardkin_feedback_copy3 by Gerhard Berman

main.cpp@2:94b5e00288a5, 2016-10-14 (annotated)

Committer:
GerhardBerman
Date:
Fri Oct 14 14:37:25 2016 +0000
Revision:
2:94b5e00288a5
Parent:
1:ace33633653b
Child:
3:8caef4872b0c
Green parts deleted

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 2:94b5e00288a5 21 Ticker MeasurePTicker;
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 2:94b5e00288a5 48 volatile bool MeasurePTicker_go=false;
GerhardBerman 1:ace33633653b 49 void MeasureP_act(){MeasurePTicker_go=true;}; // Activates go-flags
GerhardBerman 0:43160ef59f9f 50
GerhardBerman 0:43160ef59f9f 51 void MeasureP(){
GerhardBerman 0:43160ef59f9f 52 double ref_position = Potmeter1; //reference position from potmeter
GerhardBerman 0:43160ef59f9f 53 int counts = Encoder.getPulses(); // gives position
GerhardBerman 0:43160ef59f9f 54 double position = counts/resolution; //position in radians
GerhardBerman 0:43160ef59f9f 55 double rotation = ref_position-position; //rotation is 'position error' in radians
GerhardBerman 0:43160ef59f9f 56 double movement = rotation/(2*PI); //movement in rotations
GerhardBerman 0:43160ef59f9f 57 double Kp = Potmeter2;
GerhardBerman 1:ace33633653b 58
GerhardBerman 0:43160ef59f9f 59 double P_output = Kp*movement;
GerhardBerman 0:43160ef59f9f 60
GerhardBerman 0:43160ef59f9f 61 if(rotation>0){
GerhardBerman 0:43160ef59f9f 62 motor1DirectionPin.write(cw);
GerhardBerman 1:ace33633653b 63 motor1MagnitudePin.write(P_output);
GerhardBerman 0:43160ef59f9f 64 }
GerhardBerman 0:43160ef59f9f 65 if(rotation<0){
GerhardBerman 0:43160ef59f9f 66 motor1DirectionPin.write(ccw);
GerhardBerman 1:ace33633653b 67 motor1MagnitudePin.write(-(P_output));
GerhardBerman 0:43160ef59f9f 68 }
GerhardBerman 1:ace33633653b 69 pc.printf("ref_position: %d rad/s \r\n", ref_position);
GerhardBerman 1:ace33633653b 70 pc.printf("position: %d rad \r\n", position);
GerhardBerman 1:ace33633653b 71 pc.printf("rotation: %d rad \r\n", rotation);
GerhardBerman 1:ace33633653b 72 pc.printf("Kp: %d \r\n", Kp);
GerhardBerman 0:43160ef59f9f 73 }
GerhardBerman 0:43160ef59f9f 74
GerhardBerman 0:43160ef59f9f 75 int main()
GerhardBerman 0:43160ef59f9f 76 {
GerhardBerman 0:43160ef59f9f 77 //Initialize
GerhardBerman 0:43160ef59f9f 78 led1=0;
GerhardBerman 0:43160ef59f9f 79 led2=0;
GerhardBerman 2:94b5e00288a5 80 //float Potmeter1 = potMeter1.read();
GerhardBerman 2:94b5e00288a5 81 //float Potmeter2 = potMeter2.read();
GerhardBerman 1:ace33633653b 82 MeasurePTicker.attach(&MeasureP_act, 0.01f);
GerhardBerman 0:43160ef59f9f 83 pc.baud(115200);
GerhardBerman 0:43160ef59f9f 84 QEI Encoder(D12, D13, NC, 32); // turns on encoder
GerhardBerman 0:43160ef59f9f 85
GerhardBerman 0:43160ef59f9f 86 while(1)
GerhardBerman 0:43160ef59f9f 87 {
GerhardBerman 1:ace33633653b 88 if (MeasurePTicker_go){
GerhardBerman 1:ace33633653b 89 MeasurePTicker_go=false;
GerhardBerman 0:43160ef59f9f 90 MeasureP();
GerhardBerman 1:ace33633653b 91 }
GerhardBerman 0:43160ef59f9f 92 }
GerhardBerman 0:43160ef59f9f 93 }