Wouter Bos / Mbed 2 deprecated Motor_potbesturing_notgradual

motor aansturing moet lineair zijn is het niet

Dependencies:   MODSERIAL Encoder mbed HIDScope

main.cpp@18:e3b41351ee71, 2015-10-07 (annotated)

Committer:
Zeekat
Date:
Wed Oct 07 13:25:49 2015 +0000
Revision:
18:e3b41351ee71
Parent:
17:d643e5954165
Child:
19:da210f89db18
werkende versie met 1 signaal input. versterking 15

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zeekat 0:5ea1875b307a 1 #include "mbed.h"
Zeekat 17:d643e5954165 2 #include "MODSERIAL.h"
Zeekat 9:f907915f269c 3 #include "encoder.h"
Zeekat 18:e3b41351ee71 4 #include "HIDScope.h"
Zeekat 4:e171c9fa5447 5
Zeekat 17:d643e5954165 6 Serial pc(USBTX,USBRX);
Zeekat 18:e3b41351ee71 7 HIDScope scope(1); // definieerd het aantal kanalen van de scope
Zeekat 15:a90c450b1e0e 8
Zeekat 15:a90c450b1e0e 9 Ticker controller1, controller2; // definieer de ticker die controler1 doet
Zeekat 4:e171c9fa5447 10
Zeekat 0:5ea1875b307a 11
Zeekat 15:a90c450b1e0e 12 //MOTOR INPUTPINS
Zeekat 15:a90c450b1e0e 13 // motor 1
Zeekat 0:5ea1875b307a 14 PwmOut motor1_aan(D6); // PWM signaal motor 2 (uit sheets)
Zeekat 11:d31b03b05f59 15 DigitalOut motor1_rich(D7); // digitaal signaal voor richting
Zeekat 15:a90c450b1e0e 16
Zeekat 15:a90c450b1e0e 17 // motor 2
Zeekat 15:a90c450b1e0e 18 PwmOut motor2_aan(D5);
Zeekat 15:a90c450b1e0e 19 DigitalOut motor2_rich(D4);
Zeekat 15:a90c450b1e0e 20 // EINDE MOTOR
Zeekat 15:a90c450b1e0e 21
Zeekat 0:5ea1875b307a 22
Zeekat 9:f907915f269c 23 // ENCODER
Zeekat 9:f907915f269c 24 Encoder motor1_enc(D12,D11);
Zeekat 15:a90c450b1e0e 25 Encoder motor2_enc(D10,D9);
Zeekat 0:5ea1875b307a 26
Zeekat 0:5ea1875b307a 27 //POTMETERS
Zeekat 18:e3b41351ee71 28 AnalogIn potright(A0);
Zeekat 15:a90c450b1e0e 29 AnalogIn potleft(A1);
Zeekat 0:5ea1875b307a 30
Zeekat 15:a90c450b1e0e 31 // RESETBUTTON
Zeekat 15:a90c450b1e0e 32 DigitalIn button(PTA4);
Zeekat 15:a90c450b1e0e 33 int button_pressed = 0;
Zeekat 15:a90c450b1e0e 34
Zeekat 15:a90c450b1e0e 35
Zeekat 15:a90c450b1e0e 36 // controller stuff
Zeekat 18:e3b41351ee71 37 double controlfreq = 50 ; // controlloops frequentie (Hz)
Zeekat 15:a90c450b1e0e 38 double controlstep = 1/controlfreq; // timestep derived from controlfreq
Zeekat 15:a90c450b1e0e 39 const double K1 = 1 ; // P constant motorcontrolle 1
Zeekat 15:a90c450b1e0e 40 const double K2 = 1; // p constant motorcontroller 2
Zeekat 11:d31b03b05f59 41
Zeekat 16:acf850a87e01 42 // define storage variables for setpoint values
Zeekat 16:acf850a87e01 43 double c_setpoint1 = 0;
Zeekat 16:acf850a87e01 44 double c_setpoint2 = 0;
Zeekat 16:acf850a87e01 45
Zeekat 16:acf850a87e01 46 // define the maximum rate of change for the setpoint (velocity)
Zeekat 16:acf850a87e01 47 double Vmax = 5; // rad/sec
Zeekat 16:acf850a87e01 48
Zeekat 18:e3b41351ee71 49 // // // // // // // FILTEREN TOEVOEGEN
Zeekat 12:e3c5c5acbd09 50
Zeekat 18:e3b41351ee71 51 // tweede orde notch filter 50 Hz
Zeekat 18:e3b41351ee71 52 // biquad 1 coefficienten
Zeekat 18:e3b41351ee71 53 const double numnotch50biq1_1 = 1;
Zeekat 18:e3b41351ee71 54 const double numnotch50biq1_2 = -1.61816178466632;
Zeekat 18:e3b41351ee71 55 const double numnotch50biq1_3 = 1.00000006127058;
Zeekat 18:e3b41351ee71 56 const double dennotch50biq1_2 = -1.59325742941798;
Zeekat 18:e3b41351ee71 57 const double dennotch50biq1_3 = 0.982171881701431;
Zeekat 18:e3b41351ee71 58 // biquad 2 coefficienten
Zeekat 18:e3b41351ee71 59 const double numnotch50biq2_1 = 1;
Zeekat 18:e3b41351ee71 60 const double numnotch50biq2_2 = -1.61816171933244;
Zeekat 18:e3b41351ee71 61 const double numnotch50biq2_3 = 0.999999938729428;
Zeekat 18:e3b41351ee71 62 const double dennotch50biq2_2 = -1.61431180968071;
Zeekat 18:e3b41351ee71 63 const double dennotch50biq2_3 = 0.982599066293075;
Zeekat 18:e3b41351ee71 64
Zeekat 18:e3b41351ee71 65 // highpass filter 20 Hz coefficienten
Zeekat 18:e3b41351ee71 66 const double numhigh20_1 = 0.837089190566345;
Zeekat 18:e3b41351ee71 67 const double numhigh20_2 = -1.67417838113269;
Zeekat 18:e3b41351ee71 68 const double numhigh20_3 = 0.837089190566345;
Zeekat 18:e3b41351ee71 69 const double denhigh20_2 = -1.64745998107698;
Zeekat 18:e3b41351ee71 70 const double denhigh20_3 = 0.700896781188403;
Zeekat 18:e3b41351ee71 71
Zeekat 18:e3b41351ee71 72 // lowpass 5 Hz coefficienten
Zeekat 18:e3b41351ee71 73 const double numlow5_1 =0.000944691843840162;
Zeekat 18:e3b41351ee71 74 const double numlow5_2 =0.00188938368768032;
Zeekat 18:e3b41351ee71 75 const double numlow5_3 =0.000944691843840162;
Zeekat 18:e3b41351ee71 76 const double denlow5_2 =-1.91119706742607;
Zeekat 18:e3b41351ee71 77 const double denlow5_3 =0.914975834801434;
Zeekat 18:e3b41351ee71 78
Zeekat 18:e3b41351ee71 79 // Define the storage variables and filter coeicients for two filters
Zeekat 18:e3b41351ee71 80 double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0,f4_v1 = 0, f4_v2 = 0;
Zeekat 18:e3b41351ee71 81
Zeekat 18:e3b41351ee71 82 double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
Zeekat 18:e3b41351ee71 83 {
Zeekat 18:e3b41351ee71 84 double v = u- a1*v1-a2*v2;
Zeekat 18:e3b41351ee71 85 double y = b0*v+b1*v1+b2*v2;
Zeekat 18:e3b41351ee71 86 v2 = v1;
Zeekat 18:e3b41351ee71 87 v1 = v;
Zeekat 18:e3b41351ee71 88 return y;
Zeekat 18:e3b41351ee71 89 }
Zeekat 18:e3b41351ee71 90
Zeekat 18:e3b41351ee71 91 // FILTERAAR
Zeekat 18:e3b41351ee71 92 double EMG_Filter()
Zeekat 18:e3b41351ee71 93 {
Zeekat 18:e3b41351ee71 94 // double u1 = ..., u2 = ... ;
Zeekat 18:e3b41351ee71 95 double u1 = potright.read();
Zeekat 18:e3b41351ee71 96 double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
Zeekat 18:e3b41351ee71 97 double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
Zeekat 18:e3b41351ee71 98 double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
Zeekat 18:e3b41351ee71 99 double y4 = abs(y3);
Zeekat 18:e3b41351ee71 100 double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
Zeekat 18:e3b41351ee71 101 return y5;
Zeekat 18:e3b41351ee71 102 }
Zeekat 18:e3b41351ee71 103
Zeekat 18:e3b41351ee71 104
Zeekat 18:e3b41351ee71 105 // // // // // // EINDE FILTERZOOI
Zeekat 17:d643e5954165 106
Zeekat 11:d31b03b05f59 107 // counts 2 radians
Zeekat 12:e3c5c5acbd09 108 // this function takes the counts from the encoder and converts it to
Zeekat 12:e3c5c5acbd09 109 // the amount of radians from the zero position.
Zeekat 12:e3c5c5acbd09 110 double get_radians(double counts)
Zeekat 0:5ea1875b307a 111 {
Zeekat 11:d31b03b05f59 112 double pi = 3.14159265359;
Zeekat 11:d31b03b05f59 113 double radians = (counts/4200)*2*pi;
Zeekat 11:d31b03b05f59 114 return radians;
Zeekat 0:5ea1875b307a 115 }
Zeekat 0:5ea1875b307a 116
Zeekat 12:e3c5c5acbd09 117
Zeekat 12:e3c5c5acbd09 118 // this function takes a 0->1 input (in this case a potmeter and converts it
Zeekat 12:e3c5c5acbd09 119 // to a -2->2 range
Zeekat 16:acf850a87e01 120 double setpoint_f(double input, double &c_setpoint)
Zeekat 0:5ea1875b307a 121 {
Zeekat 12:e3c5c5acbd09 122 double offset = 0.5; // offset the inputsignal to -0.5->0.5
Zeekat 16:acf850a87e01 123 double gain = 2; // increase the signal
Zeekat 18:e3b41351ee71 124 // double potset = (input-offset)*gain;
Zeekat 18:e3b41351ee71 125 double setpoint = c_setpoint + input * controlstep * Vmax ;
Zeekat 17:d643e5954165 126 c_setpoint = setpoint;
Zeekat 16:acf850a87e01 127 return setpoint;
Zeekat 11:d31b03b05f59 128 }
Zeekat 11:d31b03b05f59 129
Zeekat 12:e3c5c5acbd09 130 // this function is a simple K control called by the motor function
Zeekat 15:a90c450b1e0e 131 double K_control(double error,double K)
Zeekat 11:d31b03b05f59 132 {
Zeekat 12:e3c5c5acbd09 133 double output = K*error; // controller output K*e
Zeekat 14:92614abdb7e3 134
Zeekat 14:92614abdb7e3 135 // Limit the output to to a number between -1 and 1 because
Zeekat 14:92614abdb7e3 136 // the motorcode will not handle anything smaller than -1 or larger than 1
Zeekat 14:92614abdb7e3 137 // should be put in own function to improve readability
Zeekat 14:92614abdb7e3 138 if(output>1)
Zeekat 14:92614abdb7e3 139 {
Zeekat 14:92614abdb7e3 140 output = 1;
Zeekat 14:92614abdb7e3 141 }
Zeekat 14:92614abdb7e3 142 else if(output < 1 && output > 0)
Zeekat 14:92614abdb7e3 143 {
Zeekat 14:92614abdb7e3 144 output = output;
Zeekat 14:92614abdb7e3 145 }
Zeekat 14:92614abdb7e3 146 else if(output > -1 && output < 0)
Zeekat 14:92614abdb7e3 147 {
Zeekat 14:92614abdb7e3 148 output = output;
Zeekat 14:92614abdb7e3 149 }
Zeekat 14:92614abdb7e3 150 else if(output < -1)
Zeekat 14:92614abdb7e3 151 {
Zeekat 14:92614abdb7e3 152 (output = -1);
Zeekat 14:92614abdb7e3 153 }
Zeekat 11:d31b03b05f59 154 return output;
Zeekat 11:d31b03b05f59 155 }
Zeekat 11:d31b03b05f59 156
Zeekat 14:92614abdb7e3 157 // this function controls the input for one of the electric motors and is called by a ticker
Zeekat 15:a90c450b1e0e 158 // MOTOR 1
Zeekat 15:a90c450b1e0e 159 // // // // // // // // // //
Zeekat 11:d31b03b05f59 160 void motor1_control()
Zeekat 11:d31b03b05f59 161 {
Zeekat 18:e3b41351ee71 162 double input = (EMG_Filter())*15 ;
Zeekat 18:e3b41351ee71 163 if(input > 1 )
Zeekat 18:e3b41351ee71 164 {
Zeekat 18:e3b41351ee71 165 input = 1;
Zeekat 18:e3b41351ee71 166 }
Zeekat 18:e3b41351ee71 167 else if(input < 0.2)
Zeekat 18:e3b41351ee71 168 {
Zeekat 18:e3b41351ee71 169 input = 0;
Zeekat 18:e3b41351ee71 170 }
Zeekat 18:e3b41351ee71 171 double setpoint1 = setpoint_f(input, c_setpoint1); // determine the setpoint that has been set by the inputsignal
Zeekat 15:a90c450b1e0e 172 double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor
Zeekat 15:a90c450b1e0e 173 double error1 = (setpoint1 - rads1); // determine the error (reference - position)
Zeekat 18:e3b41351ee71 174 scope.set(0,input);
Zeekat 17:d643e5954165 175 // scope.set(1,rads1);
Zeekat 18:e3b41351ee71 176 scope.send();
Zeekat 15:a90c450b1e0e 177 double output1 = K_control(error1, K1); // bereken de controller output voor motor 1(zie hierboven)
Zeekat 15:a90c450b1e0e 178 if(output1 > 0) { //
Zeekat 11:d31b03b05f59 179 motor1_rich.write(0);
Zeekat 15:a90c450b1e0e 180 motor1_aan.write(output1);
Zeekat 15:a90c450b1e0e 181 } else if(output1 < 0) {
Zeekat 11:d31b03b05f59 182 motor1_rich.write(1);
Zeekat 15:a90c450b1e0e 183 motor1_aan.write(abs(output1));
Zeekat 15:a90c450b1e0e 184 }
Zeekat 15:a90c450b1e0e 185 }
Zeekat 15:a90c450b1e0e 186
Zeekat 15:a90c450b1e0e 187 // MOTOR 2
Zeekat 15:a90c450b1e0e 188 // // // // // // //
Zeekat 15:a90c450b1e0e 189 void motor2_control()
Zeekat 15:a90c450b1e0e 190 {
Zeekat 17:d643e5954165 191 double setpoint2 = setpoint_f(potleft.read(), c_setpoint2); // determine the setpoint that has been set by the inputsignal
Zeekat 15:a90c450b1e0e 192 double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor
Zeekat 15:a90c450b1e0e 193 double error2 = (setpoint2 - rads2); // determine the error (reference - position)
Zeekat 17:d643e5954165 194 // scope.set(3,setpoint2);
Zeekat 17:d643e5954165 195 // scope.set(4,rads2);
Zeekat 17:d643e5954165 196 // scope.send();
Zeekat 15:a90c450b1e0e 197 double output2 = K_control(error2, K2); // bereken de controller output voor motor 1(zie hierboven)
Zeekat 15:a90c450b1e0e 198 if(output2 > 0) { //
Zeekat 15:a90c450b1e0e 199 motor2_rich.write(0);
Zeekat 15:a90c450b1e0e 200 motor2_aan.write(output2);
Zeekat 15:a90c450b1e0e 201 } else if(output2 < 0) {
Zeekat 15:a90c450b1e0e 202 motor2_rich.write(1);
Zeekat 15:a90c450b1e0e 203 motor2_aan.write(abs(output2));
Zeekat 11:d31b03b05f59 204 }
Zeekat 11:d31b03b05f59 205 }
Zeekat 4:e171c9fa5447 206
Zeekat 0:5ea1875b307a 207
Zeekat 0:5ea1875b307a 208 int main()
Zeekat 0:5ea1875b307a 209 {
Zeekat 15:a90c450b1e0e 210 controller1.attach(&motor1_control, controlstep);
Zeekat 15:a90c450b1e0e 211 controller2.attach(&motor2_control, controlstep);
Zeekat 11:d31b03b05f59 212 while(true)
Zeekat 0:5ea1875b307a 213 {
Zeekat 15:a90c450b1e0e 214 if(button.read() == button_pressed)
Zeekat 15:a90c450b1e0e 215 {
Zeekat 15:a90c450b1e0e 216 motor1_enc.setPosition(0);
Zeekat 15:a90c450b1e0e 217 motor2_enc.setPosition(0);
Zeekat 15:a90c450b1e0e 218 }
Zeekat 0:5ea1875b307a 219 }
Zeekat 0:5ea1875b307a 220 }