Marijke Zondag / Mbed 2 deprecated Project_script_encoder

encoder

Dependencies:   MODSERIAL mbed HIDScope biquadFilter

Fork of Project_script by Marijke Zondag

main.cpp@25:c719346df3cd, 2018-10-30 (annotated)

Committer:
MarijkeZondag
Date:
Tue Oct 30 21:27:35 2018 +0000
Revision:
25:c719346df3cd
Parent:
24:b9dd6cf5c366
Child:
26:8fc7e88603f8
WERKT NOG NIET PID, gekke weerstand, motor draait wel maar we weten niet precies wat hij doet.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:c8f15874531b 1 #include "mbed.h"
vsluiter 0:c8f15874531b 2 #include "MODSERIAL.h"
MarijkeZondag 24:b9dd6cf5c366 3 #include "BiQuad.h"
MarijkeZondag 25:c719346df3cd 4 #include "HIDScope.h"
MarijkeZondag 25:c719346df3cd 5 #include <math.h>
vsluiter 0:c8f15874531b 6
MarijkeZondag 25:c719346df3cd 7 //ATTENTION: set mBed to version 151
MarijkeZondag 25:c719346df3cd 8 // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
MarijkeZondag 25:c719346df3cd 9 // set MODSERIAL to version 44
MarijkeZondag 25:c719346df3cd 10 // set HIDScope to version 7
MarijkeZondag 25:c719346df3cd 11 // set biquadFilter to version 7
MarijkeZondag 23:c027e5c57cc8 12
MarijkeZondag 9:c722418997b5 13 InterruptIn encoderA (D9);
MarijkeZondag 9:c722418997b5 14 InterruptIn encoderB (D8);
MarijkeZondag 6:f4bbb73f3989 15
MarijkeZondag 23:c027e5c57cc8 16 DigitalOut directionpin1 (D7);
MarijkeZondag 23:c027e5c57cc8 17 DigitalOut directionpin2 (D4);
MarijkeZondag 25:c719346df3cd 18
MarijkeZondag 25:c719346df3cd 19 DigitalIn button2 (D10);
MarijkeZondag 25:c719346df3cd 20
MarijkeZondag 23:c027e5c57cc8 21 PwmOut pwmpin1 (D6);
MarijkeZondag 23:c027e5c57cc8 22 PwmOut pwmpin2 (D5);
MarijkeZondag 9:c722418997b5 23
MarijkeZondag 6:f4bbb73f3989 24
vsluiter 0:c8f15874531b 25 MODSERIAL pc(USBTX, USBRX);
MarijkeZondag 25:c719346df3cd 26 //HIDScope scope( 3 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
vsluiter 0:c8f15874531b 27
MarijkeZondag 9:c722418997b5 28
MarijkeZondag 9:c722418997b5 29 //Global variables
MarijkeZondag 25:c719346df3cd 30 int encoder = 1; //Start value encoder
MarijkeZondag 24:b9dd6cf5c366 31
MarijkeZondag 25:c719346df3cd 32 double T = 0.01f; //Ticker period
MarijkeZondag 25:c719346df3cd 33
MarijkeZondag 25:c719346df3cd 34 double Kp = 17.5; //Motor 1
MarijkeZondag 24:b9dd6cf5c366 35 double Ki = 1.02;
MarijkeZondag 24:b9dd6cf5c366 36 double Kd = 23.2;
MarijkeZondag 25:c719346df3cd 37 double PI = 3.14159;
MarijkeZondag 24:b9dd6cf5c366 38
MarijkeZondag 25:c719346df3cd 39 //double err = 0;
MarijkeZondag 25:c719346df3cd 40
MarijkeZondag 25:c719346df3cd 41 volatile double reference_rotation = PI; //pi , in radians
MarijkeZondag 25:c719346df3cd 42 double encoder_radians;
MarijkeZondag 25:c719346df3cd 43
MarijkeZondag 25:c719346df3cd 44 //double u_total = 0;
MarijkeZondag 25:c719346df3cd 45
MarijkeZondag 25:c719346df3cd 46 double pi=3.14159;
MarijkeZondag 25:c719346df3cd 47 double i=0;
MarijkeZondag 24:b9dd6cf5c366 48
MarijkeZondag 24:b9dd6cf5c366 49 //Tickers
MarijkeZondag 25:c719346df3cd 50 Ticker encoder_tick;
MarijkeZondag 25:c719346df3cd 51 Ticker engine_tick;
MarijkeZondag 8:895d941a5910 52
MarijkeZondag 9:c722418997b5 53
MarijkeZondag 9:c722418997b5 54 //Functions
MarijkeZondag 25:c719346df3cd 55
MarijkeZondag 25:c719346df3cd 56 /*
MarijkeZondag 25:c719346df3cd 57 void HIDScope_sample()
MarijkeZondag 25:c719346df3cd 58 {
MarijkeZondag 25:c719346df3cd 59 scope.set(0,sinout);
MarijkeZondag 25:c719346df3cd 60 scope.set(1,encoder); //alles in array?
MarijkeZondag 25:c719346df3cd 61 scope.set(2,err);
MarijkeZondag 25:c719346df3cd 62
MarijkeZondag 25:c719346df3cd 63 scope.send();
MarijkeZondag 25:c719346df3cd 64 }
MarijkeZondag 25:c719346df3cd 65 */
MarijkeZondag 25:c719346df3cd 66
MarijkeZondag 25:c719346df3cd 67
MarijkeZondag 8:895d941a5910 68 void encoderA_rise()
MarijkeZondag 8:895d941a5910 69 {
MarijkeZondag 8:895d941a5910 70 if(encoderB==false)
MarijkeZondag 8:895d941a5910 71 {
MarijkeZondag 8:895d941a5910 72 encoder++;
MarijkeZondag 8:895d941a5910 73 }
MarijkeZondag 8:895d941a5910 74 else
MarijkeZondag 8:895d941a5910 75 {
MarijkeZondag 8:895d941a5910 76 encoder--;
MarijkeZondag 8:895d941a5910 77 }
MarijkeZondag 8:895d941a5910 78 }
MarijkeZondag 8:895d941a5910 79
MarijkeZondag 8:895d941a5910 80 void encoderA_fall()
MarijkeZondag 8:895d941a5910 81 {
MarijkeZondag 8:895d941a5910 82 if(encoderB==true)
MarijkeZondag 8:895d941a5910 83 {
MarijkeZondag 8:895d941a5910 84 encoder++;
MarijkeZondag 8:895d941a5910 85 }
MarijkeZondag 8:895d941a5910 86 else
MarijkeZondag 8:895d941a5910 87 {
MarijkeZondag 8:895d941a5910 88 encoder--;
MarijkeZondag 8:895d941a5910 89 }
MarijkeZondag 8:895d941a5910 90 }
MarijkeZondag 8:895d941a5910 91
MarijkeZondag 8:895d941a5910 92 void encoderB_rise()
MarijkeZondag 8:895d941a5910 93 {
MarijkeZondag 8:895d941a5910 94 if(encoderA==true)
MarijkeZondag 8:895d941a5910 95 {
MarijkeZondag 8:895d941a5910 96 encoder++;
MarijkeZondag 8:895d941a5910 97 }
MarijkeZondag 8:895d941a5910 98 else
MarijkeZondag 8:895d941a5910 99 {
MarijkeZondag 8:895d941a5910 100 encoder--;
MarijkeZondag 8:895d941a5910 101 }
MarijkeZondag 8:895d941a5910 102 }
MarijkeZondag 8:895d941a5910 103
MarijkeZondag 8:895d941a5910 104 void encoderB_fall()
MarijkeZondag 8:895d941a5910 105 {
MarijkeZondag 8:895d941a5910 106 if(encoderA==false)
MarijkeZondag 8:895d941a5910 107 {
MarijkeZondag 8:895d941a5910 108 encoder++;
MarijkeZondag 8:895d941a5910 109 }
MarijkeZondag 8:895d941a5910 110 else
MarijkeZondag 8:895d941a5910 111 {
MarijkeZondag 8:895d941a5910 112 encoder--;
MarijkeZondag 8:895d941a5910 113 }
MarijkeZondag 8:895d941a5910 114 }
MarijkeZondag 8:895d941a5910 115
MarijkeZondag 24:b9dd6cf5c366 116 void encoder_count()
MarijkeZondag 24:b9dd6cf5c366 117 {
MarijkeZondag 24:b9dd6cf5c366 118 encoderA.rise(&encoderA_rise);
MarijkeZondag 24:b9dd6cf5c366 119 encoderA.fall(&encoderA_fall);
MarijkeZondag 24:b9dd6cf5c366 120 encoderB.rise(&encoderB_rise);
MarijkeZondag 24:b9dd6cf5c366 121 encoderB.fall(&encoderB_fall);
MarijkeZondag 24:b9dd6cf5c366 122 }
MarijkeZondag 24:b9dd6cf5c366 123
MarijkeZondag 25:c719346df3cd 124 double PID_controller(double err)
MarijkeZondag 24:b9dd6cf5c366 125 {
MarijkeZondag 24:b9dd6cf5c366 126 static double err_integral = 0;
MarijkeZondag 24:b9dd6cf5c366 127 static double err_prev = err; // initialization with this value only done once!
MarijkeZondag 24:b9dd6cf5c366 128
MarijkeZondag 24:b9dd6cf5c366 129 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MarijkeZondag 24:b9dd6cf5c366 130
MarijkeZondag 24:b9dd6cf5c366 131 // Proportional part:
MarijkeZondag 24:b9dd6cf5c366 132 double u_k = Kp * err;
MarijkeZondag 24:b9dd6cf5c366 133
MarijkeZondag 24:b9dd6cf5c366 134 // Integral part
MarijkeZondag 24:b9dd6cf5c366 135 err_integral = err_integral + err * T;
MarijkeZondag 24:b9dd6cf5c366 136 double u_i = Ki * err_integral;
MarijkeZondag 24:b9dd6cf5c366 137
MarijkeZondag 24:b9dd6cf5c366 138 // Derivative part
MarijkeZondag 24:b9dd6cf5c366 139 double err_derivative = (err - err_prev)/T;
MarijkeZondag 24:b9dd6cf5c366 140 double filtered_err_derivative = LowPassFilter.step(err_derivative);
MarijkeZondag 24:b9dd6cf5c366 141 double u_d = Kd * filtered_err_derivative;
MarijkeZondag 24:b9dd6cf5c366 142 err_prev = err;
MarijkeZondag 24:b9dd6cf5c366 143
MarijkeZondag 24:b9dd6cf5c366 144 // Sum all parts and return it
MarijkeZondag 25:c719346df3cd 145 return u_k + u_i + u_d;
MarijkeZondag 24:b9dd6cf5c366 146 }
MarijkeZondag 24:b9dd6cf5c366 147
MarijkeZondag 25:c719346df3cd 148 void start_your_engines(double u)
MarijkeZondag 24:b9dd6cf5c366 149 {
MarijkeZondag 25:c719346df3cd 150 if(encoder<5000 && encoder>-5000) //limieten van de translatie 11760 encoder counts
MarijkeZondag 25:c719346df3cd 151 {
MarijkeZondag 25:c719346df3cd 152 pwmpin1 = fabs(u); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
MarijkeZondag 25:c719346df3cd 153 directionpin1.write(u > 0.0f);
MarijkeZondag 25:c719346df3cd 154 }
MarijkeZondag 25:c719346df3cd 155 else
MarijkeZondag 25:c719346df3cd 156 {
MarijkeZondag 25:c719346df3cd 157 pwmpin1 = 0;
MarijkeZondag 25:c719346df3cd 158 }
MarijkeZondag 25:c719346df3cd 159 }
MarijkeZondag 25:c719346df3cd 160
MarijkeZondag 25:c719346df3cd 161 void engine_control()
MarijkeZondag 25:c719346df3cd 162 {
MarijkeZondag 25:c719346df3cd 163 encoder_radians = encoder*(2*PI)/8400;
MarijkeZondag 25:c719346df3cd 164 double err = reference_rotation - encoder_radians;
MarijkeZondag 25:c719346df3cd 165 double u = PID_controller(err); //PID controller function call
MarijkeZondag 25:c719346df3cd 166 start_your_engines(u); //Call start_your_engines function
MarijkeZondag 24:b9dd6cf5c366 167 }
MarijkeZondag 8:895d941a5910 168
MarijkeZondag 9:c722418997b5 169 // Main function start.
MarijkeZondag 8:895d941a5910 170
vsluiter 0:c8f15874531b 171 int main()
MarijkeZondag 3:a3ad58758696 172 {
MarijkeZondag 25:c719346df3cd 173 pc.baud(115200);
MarijkeZondag 25:c719346df3cd 174 pc.printf("hello\n\r");
MarijkeZondag 25:c719346df3cd 175 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 25:c719346df3cd 176
MarijkeZondag 25:c719346df3cd 177 encoder_tick.attach(&encoder_count,T);
MarijkeZondag 25:c719346df3cd 178 engine_tick.attach(&engine_control,T);
MarijkeZondag 25:c719346df3cd 179 //HIDScope ticker toevoegen
MarijkeZondag 25:c719346df3cd 180
MarijkeZondag 25:c719346df3cd 181 // DEFININING VARIABLES
MarijkeZondag 25:c719346df3cd 182
MarijkeZondag 25:c719346df3cd 183 //float reference;
MarijkeZondag 25:c719346df3cd 184 float foo[17];
MarijkeZondag 25:c719346df3cd 185 float length = 9.0f;
MarijkeZondag 25:c719346df3cd 186 int a;
MarijkeZondag 25:c719346df3cd 187 int q;
MarijkeZondag 25:c719346df3cd 188
MarijkeZondag 25:c719346df3cd 189 while (true)
MarijkeZondag 25:c719346df3cd 190 {
MarijkeZondag 6:f4bbb73f3989 191
MarijkeZondag 25:c719346df3cd 192 // EXECTURION IN MAIN
MarijkeZondag 25:c719346df3cd 193 // SIN ALS INPUT GEVEN
MarijkeZondag 25:c719346df3cd 194 if (button2 == false)
MarijkeZondag 25:c719346df3cd 195 {
MarijkeZondag 25:c719346df3cd 196 wait (0.2f);
MarijkeZondag 25:c719346df3cd 197
MarijkeZondag 25:c719346df3cd 198 for (q=0; q<4; q++)
MarijkeZondag 25:c719346df3cd 199 {
MarijkeZondag 25:c719346df3cd 200 float b = 8.0f;
MarijkeZondag 25:c719346df3cd 201
MarijkeZondag 25:c719346df3cd 202 for (a=0; a<10; a++)
MarijkeZondag 25:c719346df3cd 203 {
MarijkeZondag 25:c719346df3cd 204 foo[a] = 3.14f/length*(a);
MarijkeZondag 25:c719346df3cd 205 //printf("%f\n\r", foo[a]);
MarijkeZondag 25:c719346df3cd 206 reference_rotation = foo[a];
MarijkeZondag 25:c719346df3cd 207 wait(0.2f);
MarijkeZondag 25:c719346df3cd 208 }
MarijkeZondag 25:c719346df3cd 209
MarijkeZondag 25:c719346df3cd 210 for(a=10; a<18; a++)
MarijkeZondag 25:c719346df3cd 211 {
MarijkeZondag 25:c719346df3cd 212 foo[a] = 3.14f/length*b;
MarijkeZondag 25:c719346df3cd 213 //printf("%f\n\r", foo[a]);
MarijkeZondag 25:c719346df3cd 214 reference_rotation = foo[a];
MarijkeZondag 25:c719346df3cd 215 b = b-1.0f;
MarijkeZondag 25:c719346df3cd 216 wait(0.2f);
MarijkeZondag 25:c719346df3cd 217 }
MarijkeZondag 25:c719346df3cd 218 }
MarijkeZondag 25:c719346df3cd 219
MarijkeZondag 25:c719346df3cd 220 reference_rotation = 0;
MarijkeZondag 7:f32005d13749 221
MarijkeZondag 25:c719346df3cd 222 }
MarijkeZondag 24:b9dd6cf5c366 223
vsluiter 0:c8f15874531b 224 }
MarijkeZondag 25:c719346df3cd 225 }