Marijke Zondag / Mbed 2 deprecated Project_script_encoder

encoder

Dependencies:   MODSERIAL mbed HIDScope biquadFilter

Fork of Project_script by Marijke Zondag

main.cpp@26:8fc7e88603f8, 2018-11-02 (annotated)

Committer:
MarijkeZondag
Date:
Fri Nov 02 07:58:00 2018 +0000
Revision:
26:8fc7e88603f8
Parent:
25:c719346df3cd 
,.

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 i=0;
MarijkeZondag 24:b9dd6cf5c366 47
MarijkeZondag 24:b9dd6cf5c366 48 //Tickers
MarijkeZondag 25:c719346df3cd 49 Ticker encoder_tick;
MarijkeZondag 25:c719346df3cd 50 Ticker engine_tick;
MarijkeZondag 8:895d941a5910 51
MarijkeZondag 9:c722418997b5 52
MarijkeZondag 9:c722418997b5 53 //Functions
MarijkeZondag 25:c719346df3cd 54
MarijkeZondag 25:c719346df3cd 55 /*
MarijkeZondag 25:c719346df3cd 56 void HIDScope_sample()
MarijkeZondag 25:c719346df3cd 57 {
MarijkeZondag 25:c719346df3cd 58 scope.set(0,sinout);
MarijkeZondag 25:c719346df3cd 59 scope.set(1,encoder); //alles in array?
MarijkeZondag 25:c719346df3cd 60 scope.set(2,err);
MarijkeZondag 25:c719346df3cd 61
MarijkeZondag 25:c719346df3cd 62 scope.send();
MarijkeZondag 25:c719346df3cd 63 }
MarijkeZondag 25:c719346df3cd 64 */
MarijkeZondag 25:c719346df3cd 65
MarijkeZondag 25:c719346df3cd 66
MarijkeZondag 8:895d941a5910 67 void encoderA_rise()
MarijkeZondag 8:895d941a5910 68 {
MarijkeZondag 8:895d941a5910 69 if(encoderB==false)
MarijkeZondag 8:895d941a5910 70 {
MarijkeZondag 8:895d941a5910 71 encoder++;
MarijkeZondag 8:895d941a5910 72 }
MarijkeZondag 8:895d941a5910 73 else
MarijkeZondag 8:895d941a5910 74 {
MarijkeZondag 8:895d941a5910 75 encoder--;
MarijkeZondag 8:895d941a5910 76 }
MarijkeZondag 8:895d941a5910 77 }
MarijkeZondag 8:895d941a5910 78
MarijkeZondag 8:895d941a5910 79 void encoderA_fall()
MarijkeZondag 8:895d941a5910 80 {
MarijkeZondag 8:895d941a5910 81 if(encoderB==true)
MarijkeZondag 8:895d941a5910 82 {
MarijkeZondag 8:895d941a5910 83 encoder++;
MarijkeZondag 8:895d941a5910 84 }
MarijkeZondag 8:895d941a5910 85 else
MarijkeZondag 8:895d941a5910 86 {
MarijkeZondag 8:895d941a5910 87 encoder--;
MarijkeZondag 8:895d941a5910 88 }
MarijkeZondag 8:895d941a5910 89 }
MarijkeZondag 8:895d941a5910 90
MarijkeZondag 8:895d941a5910 91 void encoderB_rise()
MarijkeZondag 8:895d941a5910 92 {
MarijkeZondag 8:895d941a5910 93 if(encoderA==true)
MarijkeZondag 8:895d941a5910 94 {
MarijkeZondag 8:895d941a5910 95 encoder++;
MarijkeZondag 8:895d941a5910 96 }
MarijkeZondag 8:895d941a5910 97 else
MarijkeZondag 8:895d941a5910 98 {
MarijkeZondag 8:895d941a5910 99 encoder--;
MarijkeZondag 8:895d941a5910 100 }
MarijkeZondag 8:895d941a5910 101 }
MarijkeZondag 8:895d941a5910 102
MarijkeZondag 8:895d941a5910 103 void encoderB_fall()
MarijkeZondag 8:895d941a5910 104 {
MarijkeZondag 8:895d941a5910 105 if(encoderA==false)
MarijkeZondag 8:895d941a5910 106 {
MarijkeZondag 8:895d941a5910 107 encoder++;
MarijkeZondag 8:895d941a5910 108 }
MarijkeZondag 8:895d941a5910 109 else
MarijkeZondag 8:895d941a5910 110 {
MarijkeZondag 8:895d941a5910 111 encoder--;
MarijkeZondag 8:895d941a5910 112 }
MarijkeZondag 8:895d941a5910 113 }
MarijkeZondag 8:895d941a5910 114
MarijkeZondag 24:b9dd6cf5c366 115 void encoder_count()
MarijkeZondag 24:b9dd6cf5c366 116 {
MarijkeZondag 24:b9dd6cf5c366 117 encoderA.rise(&encoderA_rise);
MarijkeZondag 24:b9dd6cf5c366 118 encoderA.fall(&encoderA_fall);
MarijkeZondag 24:b9dd6cf5c366 119 encoderB.rise(&encoderB_rise);
MarijkeZondag 24:b9dd6cf5c366 120 encoderB.fall(&encoderB_fall);
MarijkeZondag 24:b9dd6cf5c366 121 }
MarijkeZondag 24:b9dd6cf5c366 122
MarijkeZondag 25:c719346df3cd 123 double PID_controller(double err)
MarijkeZondag 24:b9dd6cf5c366 124 {
MarijkeZondag 24:b9dd6cf5c366 125 static double err_integral = 0;
MarijkeZondag 24:b9dd6cf5c366 126 static double err_prev = err; // initialization with this value only done once!
MarijkeZondag 24:b9dd6cf5c366 127
MarijkeZondag 24:b9dd6cf5c366 128 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MarijkeZondag 24:b9dd6cf5c366 129
MarijkeZondag 24:b9dd6cf5c366 130 // Proportional part:
MarijkeZondag 24:b9dd6cf5c366 131 double u_k = Kp * err;
MarijkeZondag 24:b9dd6cf5c366 132
MarijkeZondag 24:b9dd6cf5c366 133 // Integral part
MarijkeZondag 24:b9dd6cf5c366 134 err_integral = err_integral + err * T;
MarijkeZondag 24:b9dd6cf5c366 135 double u_i = Ki * err_integral;
MarijkeZondag 24:b9dd6cf5c366 136
MarijkeZondag 24:b9dd6cf5c366 137 // Derivative part
MarijkeZondag 24:b9dd6cf5c366 138 double err_derivative = (err - err_prev)/T;
MarijkeZondag 24:b9dd6cf5c366 139 double filtered_err_derivative = LowPassFilter.step(err_derivative);
MarijkeZondag 24:b9dd6cf5c366 140 double u_d = Kd * filtered_err_derivative;
MarijkeZondag 24:b9dd6cf5c366 141 err_prev = err;
MarijkeZondag 24:b9dd6cf5c366 142
MarijkeZondag 24:b9dd6cf5c366 143 // Sum all parts and return it
MarijkeZondag 25:c719346df3cd 144 return u_k + u_i + u_d;
MarijkeZondag 24:b9dd6cf5c366 145 }
MarijkeZondag 24:b9dd6cf5c366 146
MarijkeZondag 25:c719346df3cd 147 void start_your_engines(double u)
MarijkeZondag 24:b9dd6cf5c366 148 {
MarijkeZondag 25:c719346df3cd 149 if(encoder<5000 && encoder>-5000) //limieten van de translatie 11760 encoder counts
MarijkeZondag 25:c719346df3cd 150 {
MarijkeZondag 25:c719346df3cd 151 pwmpin1 = fabs(u); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
MarijkeZondag 26:8fc7e88603f8 152 directionpin1.write(u < 0.0f);
MarijkeZondag 25:c719346df3cd 153 }
MarijkeZondag 25:c719346df3cd 154 else
MarijkeZondag 25:c719346df3cd 155 {
MarijkeZondag 25:c719346df3cd 156 pwmpin1 = 0;
MarijkeZondag 25:c719346df3cd 157 }
MarijkeZondag 25:c719346df3cd 158 }
MarijkeZondag 25:c719346df3cd 159
MarijkeZondag 25:c719346df3cd 160 void engine_control()
MarijkeZondag 25:c719346df3cd 161 {
MarijkeZondag 25:c719346df3cd 162 encoder_radians = encoder*(2*PI)/8400;
MarijkeZondag 25:c719346df3cd 163 double err = reference_rotation - encoder_radians;
MarijkeZondag 25:c719346df3cd 164 double u = PID_controller(err); //PID controller function call
MarijkeZondag 25:c719346df3cd 165 start_your_engines(u); //Call start_your_engines function
MarijkeZondag 24:b9dd6cf5c366 166 }
MarijkeZondag 8:895d941a5910 167
MarijkeZondag 9:c722418997b5 168 // Main function start.
MarijkeZondag 8:895d941a5910 169
vsluiter 0:c8f15874531b 170 int main()
MarijkeZondag 3:a3ad58758696 171 {
MarijkeZondag 25:c719346df3cd 172 pc.baud(115200);
MarijkeZondag 25:c719346df3cd 173 pc.printf("hello\n\r");
MarijkeZondag 25:c719346df3cd 174 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 25:c719346df3cd 175
MarijkeZondag 25:c719346df3cd 176 encoder_tick.attach(&encoder_count,T);
MarijkeZondag 25:c719346df3cd 177 engine_tick.attach(&engine_control,T);
MarijkeZondag 25:c719346df3cd 178 //HIDScope ticker toevoegen
MarijkeZondag 25:c719346df3cd 179
MarijkeZondag 25:c719346df3cd 180 // DEFININING VARIABLES
MarijkeZondag 25:c719346df3cd 181
MarijkeZondag 25:c719346df3cd 182 //float reference;
MarijkeZondag 25:c719346df3cd 183 float foo[17];
MarijkeZondag 25:c719346df3cd 184 float length = 9.0f;
MarijkeZondag 25:c719346df3cd 185 int a;
MarijkeZondag 25:c719346df3cd 186 int q;
MarijkeZondag 25:c719346df3cd 187
MarijkeZondag 25:c719346df3cd 188 while (true)
MarijkeZondag 25:c719346df3cd 189 {
MarijkeZondag 6:f4bbb73f3989 190
MarijkeZondag 25:c719346df3cd 191 // EXECTURION IN MAIN
MarijkeZondag 25:c719346df3cd 192 // SIN ALS INPUT GEVEN
MarijkeZondag 25:c719346df3cd 193 if (button2 == false)
MarijkeZondag 25:c719346df3cd 194 {
MarijkeZondag 25:c719346df3cd 195 wait (0.2f);
MarijkeZondag 25:c719346df3cd 196
MarijkeZondag 25:c719346df3cd 197 for (q=0; q<4; q++)
MarijkeZondag 25:c719346df3cd 198 {
MarijkeZondag 25:c719346df3cd 199 float b = 8.0f;
MarijkeZondag 25:c719346df3cd 200
MarijkeZondag 25:c719346df3cd 201 for (a=0; a<10; a++)
MarijkeZondag 25:c719346df3cd 202 {
MarijkeZondag 25:c719346df3cd 203 foo[a] = 3.14f/length*(a);
MarijkeZondag 25:c719346df3cd 204 //printf("%f\n\r", foo[a]);
MarijkeZondag 25:c719346df3cd 205 reference_rotation = foo[a];
MarijkeZondag 25:c719346df3cd 206 wait(0.2f);
MarijkeZondag 25:c719346df3cd 207 }
MarijkeZondag 25:c719346df3cd 208
MarijkeZondag 25:c719346df3cd 209 for(a=10; a<18; a++)
MarijkeZondag 25:c719346df3cd 210 {
MarijkeZondag 25:c719346df3cd 211 foo[a] = 3.14f/length*b;
MarijkeZondag 25:c719346df3cd 212 //printf("%f\n\r", foo[a]);
MarijkeZondag 25:c719346df3cd 213 reference_rotation = foo[a];
MarijkeZondag 25:c719346df3cd 214 b = b-1.0f;
MarijkeZondag 25:c719346df3cd 215 wait(0.2f);
MarijkeZondag 25:c719346df3cd 216 }
MarijkeZondag 25:c719346df3cd 217 }
MarijkeZondag 25:c719346df3cd 218
MarijkeZondag 25:c719346df3cd 219 reference_rotation = 0;
MarijkeZondag 7:f32005d13749 220
MarijkeZondag 25:c719346df3cd 221 }
MarijkeZondag 24:b9dd6cf5c366 222
vsluiter 0:c8f15874531b 223 }
MarijkeZondag 25:c719346df3cd 224 }