Willem Hoitzing / Mbed 2 deprecated PID_controller

PID controller voor 1 motor die een hoek van 20 graden draait, niet werkend.

Dependencies:   MODSERIAL QEI mbed biquadFilter

Inverse Kinematics + PID Controller

main.cpp@12:ff7947a79149, 2016-10-28 (annotated)

Committer:
willem_hoitzing
Date:
Fri Oct 28 14:17:12 2016 +0000
Revision:
12:ff7947a79149
Parent:
11:03d979f1517f
Child:
13:fe2efec19eda
Referentiehoeken gefixt: stuurt aan op vorige referentiehoek.; werkt!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
willem_hoitzing 0:26ce65a63616 1 #include "stdio.h"
willem_hoitzing 0:26ce65a63616 2 #include "math.h"
willem_hoitzing 0:26ce65a63616 3 #include "mbed.h"
willem_hoitzing 0:26ce65a63616 4 #include "QEI.h"
willem_hoitzing 0:26ce65a63616 5 #include "MODSERIAL.h"
willem_hoitzing 3:6ba52d1ae499 6 #include "BiQuad.h"
willem_hoitzing 0:26ce65a63616 7
willem_hoitzing 0:26ce65a63616 8 MODSERIAL pc(USBTX, USBRX);
willem_hoitzing 2:0a976fb06ff8 9 QEI wheel_M1 (D13, D12, NC, 32);
willem_hoitzing 2:0a976fb06ff8 10 QEI wheel_M2 (D10, D11, NC, 32);
willem_hoitzing 2:0a976fb06ff8 11 PwmOut pwm_M1 (D6);
willem_hoitzing 2:0a976fb06ff8 12 PwmOut pwm_M2 (D5);
willem_hoitzing 2:0a976fb06ff8 13 DigitalOut dir_M1 (D7);
willem_hoitzing 2:0a976fb06ff8 14 DigitalOut dir_M2 (D4);
willem_hoitzing 7:1444604f10d4 15
willem_hoitzing 7:1444604f10d4 16 DigitalOut ledg (LED_GREEN);
willem_hoitzing 7:1444604f10d4 17 DigitalOut ledr (LED_RED);
willem_hoitzing 7:1444604f10d4 18 DigitalOut ledb (LED_BLUE);
willem_hoitzing 7:1444604f10d4 19 InterruptIn knop_biceps(SW2);
willem_hoitzing 7:1444604f10d4 20 InterruptIn knop_triceps(SW3);
willem_hoitzing 7:1444604f10d4 21 InterruptIn knop_switch(D9);
willem_hoitzing 0:26ce65a63616 22
willem_hoitzing 9:334b1596637b 23 volatile float q1 = 0;
willem_hoitzing 9:334b1596637b 24 volatile float q2 = 0;
willem_hoitzing 9:334b1596637b 25 volatile float q1_begin;
willem_hoitzing 9:334b1596637b 26 volatile float q2_begin;
willem_hoitzing 11:03d979f1517f 27 const float l1 = 0.3626;
willem_hoitzing 11:03d979f1517f 28 const float l2 = 0.420;
willem_hoitzing 9:334b1596637b 29 volatile float q1_v;
willem_hoitzing 9:334b1596637b 30 volatile float q2_v;
willem_hoitzing 10:f60f9849980a 31 volatile float q1_ref = 0;
willem_hoitzing 10:f60f9849980a 32 volatile float q2_ref = 0;
willem_hoitzing 12:ff7947a79149 33 volatile float q1_ref_prev = 0;
willem_hoitzing 12:ff7947a79149 34 volatile float q2_ref_prev = 0;
willem_hoitzing 10:f60f9849980a 35 volatile float q1_error = 0;
willem_hoitzing 10:f60f9849980a 36 volatile float q2_error = 0;
willem_hoitzing 10:f60f9849980a 37 volatile float q1_error_prev = 0;
willem_hoitzing 10:f60f9849980a 38 volatile float q2_error_prev = 0;
willem_hoitzing 10:f60f9849980a 39 volatile float q1DerivativeError = 0;
willem_hoitzing 10:f60f9849980a 40 volatile float q2DerivativeError = 0;
willem_hoitzing 10:f60f9849980a 41 volatile float q1IntError = 0;
willem_hoitzing 10:f60f9849980a 42 volatile float q2IntError = 0;
willem_hoitzing 12:ff7947a79149 43 volatile float q1_total_error= 0;
willem_hoitzing 12:ff7947a79149 44 volatile float q2_total_error= 0;
willem_hoitzing 10:f60f9849980a 45 float motorValue1Out = 0.0;
willem_hoitzing 10:f60f9849980a 46 float motorValue2Out = 0.0;
willem_hoitzing 9:334b1596637b 47 volatile float ctrlOutput_M1 = 0;
willem_hoitzing 9:334b1596637b 48 volatile float ctrlOutput_M2 = 0;
willem_hoitzing 9:334b1596637b 49 volatile float vx;
willem_hoitzing 9:334b1596637b 50 volatile float vy;
willem_hoitzing 7:1444604f10d4 51 volatile bool translatie_richting = true; //true is verticaal, false is horizontaal
willem_hoitzing 0:26ce65a63616 52
willem_hoitzing 9:334b1596637b 53 const float TS = 0.02;
willem_hoitzing 11:03d979f1517f 54 const float MotorGain_M1 = 4.3; // bij pwm = 1 draait (losse) motor met 4.3 rad/s -> gemeten
willem_hoitzing 11:03d979f1517f 55 const float MotorGain_M2 = 4.7; // gemeten
willem_hoitzing 3:6ba52d1ae499 56
willem_hoitzing 4:a5f3e1838e3e 57 Ticker update_encoder_ticker;
willem_hoitzing 5:0251fde34cdc 58 volatile bool go_flag_update_encoder = false;
willem_hoitzing 5:0251fde34cdc 59 void flag_update_encoder()
willem_hoitzing 5:0251fde34cdc 60 {
willem_hoitzing 5:0251fde34cdc 61 go_flag_update_encoder = true;
willem_hoitzing 5:0251fde34cdc 62 }
willem_hoitzing 5:0251fde34cdc 63
willem_hoitzing 4:a5f3e1838e3e 64 void update_encoder()
willem_hoitzing 2:0a976fb06ff8 65 {
willem_hoitzing 10:f60f9849980a 66 //q1 = wheel_M1.getPulses()/(1334.355/2);
willem_hoitzing 10:f60f9849980a 67 //q2 = wheel_M2.getPulses()/(1334.355/2);
willem_hoitzing 12:ff7947a79149 68 //pc.printf("q1 = %f \tq1_ref = %f \tq2 = %f \tq2_ref = %f \ttotalerr1 = %f \ttotalerr2 = %f\n\r",q1, q1_ref,q2,q2_ref,q1_total_error,q2_total_error);
willem_hoitzing 12:ff7947a79149 69 //pc.printf("vx = %f \tvy = %f \tq1_v = %f \tq2_v = %f \tq1 = %f \tq2 = %f \tPID_M1 = %f \tPID_M2 = %f\n\r",vx,vy,q1_v,q2_v,q1,q2,ctrlOutput_M1,ctrlOutput_M2);
willem_hoitzing 12:ff7947a79149 70 //pc.printf("q1_err = %0.9f \tq2_err = %0.9f \tq1IntErr = %0.9f \tq2IntErr = %0.9f \tTotErr1 = %0.9f \tTotErr2 = %0.9f\n\r",q1_error,q2_error,q1IntError,q2IntError,q1_total_error,q2_total_error);
willem_hoitzing 7:1444604f10d4 71 }
willem_hoitzing 7:1444604f10d4 72
willem_hoitzing 7:1444604f10d4 73 volatile bool go_flag_initialize = false;
willem_hoitzing 7:1444604f10d4 74
willem_hoitzing 7:1444604f10d4 75 void flag_initialize()
willem_hoitzing 7:1444604f10d4 76 {
willem_hoitzing 7:1444604f10d4 77 go_flag_initialize = true;
willem_hoitzing 7:1444604f10d4 78 }
willem_hoitzing 7:1444604f10d4 79
willem_hoitzing 9:334b1596637b 80 Ticker PIDcontrol;
willem_hoitzing 9:334b1596637b 81 volatile bool go_flag_controller = false;
willem_hoitzing 9:334b1596637b 82
willem_hoitzing 9:334b1596637b 83 void flag_controller()
willem_hoitzing 9:334b1596637b 84 {
willem_hoitzing 9:334b1596637b 85 go_flag_controller = true;
willem_hoitzing 9:334b1596637b 86 }
willem_hoitzing 9:334b1596637b 87
willem_hoitzing 9:334b1596637b 88 volatile bool active_PID_ticker = false;
willem_hoitzing 9:334b1596637b 89
willem_hoitzing 9:334b1596637b 90 void begin_hoeken()
willem_hoitzing 9:334b1596637b 91 {
willem_hoitzing 9:334b1596637b 92 wait(3);
willem_hoitzing 10:f60f9849980a 93 q1_ref = wheel_M1.getPulses()/(1334.355/2);
willem_hoitzing 10:f60f9849980a 94 q2_ref = wheel_M2.getPulses()/(1334.355/2);
willem_hoitzing 9:334b1596637b 95 active_PID_ticker = true;
willem_hoitzing 9:334b1596637b 96 }
willem_hoitzing 9:334b1596637b 97
willem_hoitzing 9:334b1596637b 98
willem_hoitzing 7:1444604f10d4 99 void initialize()
willem_hoitzing 7:1444604f10d4 100 {
willem_hoitzing 9:334b1596637b 101 dir_M1 = 0; //ccw
willem_hoitzing 9:334b1596637b 102 dir_M2 = 1; //cw
willem_hoitzing 11:03d979f1517f 103 while (q1 < 40*2*3.1415/360) {
willem_hoitzing 9:334b1596637b 104 q1 = wheel_M1.getPulses()/(1334.355/2);
willem_hoitzing 11:03d979f1517f 105 pwm_M1 = 0.05;
willem_hoitzing 11:03d979f1517f 106 wait(0.005f);
willem_hoitzing 9:334b1596637b 107 }
willem_hoitzing 9:334b1596637b 108 pwm_M1 = 0;
willem_hoitzing 9:334b1596637b 109
willem_hoitzing 12:ff7947a79149 110 while (q2 > -90*2*3.1415/360) {
willem_hoitzing 9:334b1596637b 111 q2 = wheel_M2.getPulses()/(1334.355/2);
willem_hoitzing 11:03d979f1517f 112 pwm_M2 = 0.05;
willem_hoitzing 11:03d979f1517f 113 wait(0.005f);
willem_hoitzing 9:334b1596637b 114 }
willem_hoitzing 9:334b1596637b 115 pwm_M2 = 0;
willem_hoitzing 9:334b1596637b 116 begin_hoeken();
willem_hoitzing 7:1444604f10d4 117 }
willem_hoitzing 7:1444604f10d4 118
willem_hoitzing 9:334b1596637b 119
willem_hoitzing 7:1444604f10d4 120 void biceps()
willem_hoitzing 7:1444604f10d4 121 {
willem_hoitzing 12:ff7947a79149 122 q1_ref_prev = 0;
willem_hoitzing 12:ff7947a79149 123 q2_ref_prev = 0;
willem_hoitzing 10:f60f9849980a 124 q1IntError = 0;
willem_hoitzing 10:f60f9849980a 125 q2IntError = 0;
willem_hoitzing 10:f60f9849980a 126 q1_error_prev = 0;
willem_hoitzing 10:f60f9849980a 127 q2_error_prev = 0;
willem_hoitzing 11:03d979f1517f 128 if (translatie_richting == true) { // verticaal / up / blauw
willem_hoitzing 7:1444604f10d4 129 vx = 0;
willem_hoitzing 10:f60f9849980a 130 vy = 0.1;
willem_hoitzing 11:03d979f1517f 131 } else { // horizontaal / right / rood
willem_hoitzing 10:f60f9849980a 132 vx = 0.1;
willem_hoitzing 7:1444604f10d4 133 vy = 0;
willem_hoitzing 7:1444604f10d4 134 }
willem_hoitzing 7:1444604f10d4 135 }
willem_hoitzing 7:1444604f10d4 136
willem_hoitzing 7:1444604f10d4 137 void triceps()
willem_hoitzing 7:1444604f10d4 138 {
willem_hoitzing 12:ff7947a79149 139 q1_ref_prev = 0;
willem_hoitzing 12:ff7947a79149 140 q2_ref_prev = 0;
willem_hoitzing 10:f60f9849980a 141 q1IntError = 0;
willem_hoitzing 10:f60f9849980a 142 q2IntError = 0;
willem_hoitzing 10:f60f9849980a 143 q1_error_prev = 0;
willem_hoitzing 10:f60f9849980a 144 q2_error_prev = 0;
willem_hoitzing 11:03d979f1517f 145 if (translatie_richting == true) { // verticaal / down / blauw
willem_hoitzing 7:1444604f10d4 146 vx = 0;
willem_hoitzing 10:f60f9849980a 147 vy = -0.1;
willem_hoitzing 11:03d979f1517f 148 } else { // horizontaal / left / rood
willem_hoitzing 10:f60f9849980a 149 vx = -0.1;
willem_hoitzing 7:1444604f10d4 150 vy = 0;
willem_hoitzing 7:1444604f10d4 151 }
willem_hoitzing 7:1444604f10d4 152
willem_hoitzing 7:1444604f10d4 153 }
willem_hoitzing 7:1444604f10d4 154
willem_hoitzing 7:1444604f10d4 155 void switcher()
willem_hoitzing 7:1444604f10d4 156 {
willem_hoitzing 10:f60f9849980a 157 if ( (vx == 0) && (vy == 0) && (translatie_richting == true) ) {
willem_hoitzing 10:f60f9849980a 158 translatie_richting = false;
willem_hoitzing 10:f60f9849980a 159 } else if ( (vx == 0) && (vy == 0) && (translatie_richting == false) ) {
willem_hoitzing 10:f60f9849980a 160 translatie_richting = true;
willem_hoitzing 7:1444604f10d4 161 } else {
willem_hoitzing 7:1444604f10d4 162 vx = 0;
willem_hoitzing 7:1444604f10d4 163 vy = 0;
willem_hoitzing 12:ff7947a79149 164 q1_ref = q1;
willem_hoitzing 12:ff7947a79149 165 q2_ref = q2;
willem_hoitzing 12:ff7947a79149 166 q1_error = 0;
willem_hoitzing 12:ff7947a79149 167 q2_error = 0;
willem_hoitzing 10:f60f9849980a 168 q1IntError = 0;
willem_hoitzing 10:f60f9849980a 169 q2IntError = 0;
willem_hoitzing 10:f60f9849980a 170 q1_error_prev = 0;
willem_hoitzing 10:f60f9849980a 171 q2_error_prev = 0;
willem_hoitzing 12:ff7947a79149 172 q1_total_error = 0;
willem_hoitzing 12:ff7947a79149 173 q1_total_error = 0;
willem_hoitzing 12:ff7947a79149 174 pc.printf("\n\rSWITCH!!! \n\n\r");
willem_hoitzing 7:1444604f10d4 175 }
willem_hoitzing 7:1444604f10d4 176
willem_hoitzing 7:1444604f10d4 177 if (translatie_richting == 1) {
willem_hoitzing 7:1444604f10d4 178 ledr = 1; // blauw - verticaal
willem_hoitzing 7:1444604f10d4 179 ledg = 1;
willem_hoitzing 7:1444604f10d4 180 ledb = 0;
willem_hoitzing 7:1444604f10d4 181 } else {
willem_hoitzing 7:1444604f10d4 182 ledr = 0; // rood - horizontaal
willem_hoitzing 7:1444604f10d4 183 ledg = 1;
willem_hoitzing 7:1444604f10d4 184 ledb = 1;
willem_hoitzing 7:1444604f10d4 185 }
willem_hoitzing 7:1444604f10d4 186 }
willem_hoitzing 7:1444604f10d4 187
willem_hoitzing 7:1444604f10d4 188 Ticker update_ref_ticker;
willem_hoitzing 9:334b1596637b 189 volatile float J_1;
willem_hoitzing 9:334b1596637b 190 volatile float J_2;
willem_hoitzing 9:334b1596637b 191 volatile float J_3;
willem_hoitzing 9:334b1596637b 192 volatile float J_4;
willem_hoitzing 7:1444604f10d4 193 volatile bool go_flag_update_ref = false;
willem_hoitzing 7:1444604f10d4 194 void flag_update_ref()
willem_hoitzing 7:1444604f10d4 195 {
willem_hoitzing 7:1444604f10d4 196 go_flag_update_ref = true;
willem_hoitzing 7:1444604f10d4 197 }
willem_hoitzing 7:1444604f10d4 198
willem_hoitzing 7:1444604f10d4 199 void update_ref()
willem_hoitzing 7:1444604f10d4 200 {
willem_hoitzing 7:1444604f10d4 201 q1 = wheel_M1.getPulses() / (1334.355/2); // rad
willem_hoitzing 7:1444604f10d4 202 q2 = wheel_M2.getPulses() / (1334.355/2);
willem_hoitzing 7:1444604f10d4 203
willem_hoitzing 7:1444604f10d4 204 J_1 = -(l2*sin(q1 + q2))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
willem_hoitzing 7:1444604f10d4 205 J_2 = (l2*cos(q1 + q2))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
willem_hoitzing 7:1444604f10d4 206 J_3 = (l2*sin(q1 + q2) + l1*sin(q1))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
willem_hoitzing 7:1444604f10d4 207 J_4 = -(l2*cos(q1 + q2) + l1*cos(q1))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
willem_hoitzing 7:1444604f10d4 208
willem_hoitzing 7:1444604f10d4 209 q1_v = J_1 * vx + J_2 * vy;
willem_hoitzing 7:1444604f10d4 210 q2_v = J_3 * vx + J_4 * vy;
willem_hoitzing 7:1444604f10d4 211
willem_hoitzing 12:ff7947a79149 212 //pc.printf("q1 = %f \tq2 = %f \tq1_v = %f \tq2_v = %f\n\r", q1,q2,q1_v,q2_v);
willem_hoitzing 12:ff7947a79149 213
willem_hoitzing 7:1444604f10d4 214 if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) { // WAARDES VINDEN 0.8726 (50 graden)
willem_hoitzing 7:1444604f10d4 215 q1_v = 0;
willem_hoitzing 7:1444604f10d4 216 q2_v = 0;
willem_hoitzing 12:ff7947a79149 217 q1_ref_prev = 0;
willem_hoitzing 12:ff7947a79149 218 q2_ref_prev = 0;
willem_hoitzing 12:ff7947a79149 219 q1IntError = 0;
willem_hoitzing 12:ff7947a79149 220 q2IntError = 0;
willem_hoitzing 12:ff7947a79149 221 q1_error_prev = 0;
willem_hoitzing 12:ff7947a79149 222 q2_error_prev = 0;
willem_hoitzing 7:1444604f10d4 223 } else if ( (q1 < -(90*2*3.1415/360)) && (q1_v < 0) ) {
willem_hoitzing 7:1444604f10d4 224 q1_v = 0;
willem_hoitzing 7:1444604f10d4 225 q2_v = 0;
willem_hoitzing 12:ff7947a79149 226 q1_ref_prev = 0;
willem_hoitzing 12:ff7947a79149 227 q2_ref_prev = 0;
willem_hoitzing 12:ff7947a79149 228 q1IntError = 0;
willem_hoitzing 12:ff7947a79149 229 q2IntError = 0;
willem_hoitzing 12:ff7947a79149 230 q1_error_prev = 0;
willem_hoitzing 12:ff7947a79149 231 q2_error_prev = 0;
willem_hoitzing 7:1444604f10d4 232 } else if ( (q2 < (-140*2*3.1415/360)) && (q2_v < 0) ) { // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452
willem_hoitzing 7:1444604f10d4 233 q1_v = 0;
willem_hoitzing 7:1444604f10d4 234 q2_v = 0;
willem_hoitzing 12:ff7947a79149 235 q1_ref_prev = 0;
willem_hoitzing 12:ff7947a79149 236 q2_ref_prev = 0;
willem_hoitzing 12:ff7947a79149 237 q1IntError = 0;
willem_hoitzing 12:ff7947a79149 238 q2IntError = 0;
willem_hoitzing 12:ff7947a79149 239 q1_error_prev = 0;
willem_hoitzing 12:ff7947a79149 240 q2_error_prev = 0;
willem_hoitzing 12:ff7947a79149 241 } else if ( (q2 >= 0) && (q2_v > 0) ) {
willem_hoitzing 7:1444604f10d4 242 q1_v = 0;
willem_hoitzing 7:1444604f10d4 243 q2_v = 0;
willem_hoitzing 12:ff7947a79149 244 q1_ref_prev = 0;
willem_hoitzing 12:ff7947a79149 245 q2_ref_prev = 0;
willem_hoitzing 12:ff7947a79149 246 q1IntError = 0;
willem_hoitzing 12:ff7947a79149 247 q2IntError = 0;
willem_hoitzing 12:ff7947a79149 248 q1_error_prev = 0;
willem_hoitzing 12:ff7947a79149 249 q2_error_prev = 0;
willem_hoitzing 7:1444604f10d4 250 }
willem_hoitzing 12:ff7947a79149 251
willem_hoitzing 12:ff7947a79149 252 q1_ref_prev = q1_ref;
willem_hoitzing 12:ff7947a79149 253 q2_ref_prev = q2_ref;
willem_hoitzing 12:ff7947a79149 254
willem_hoitzing 12:ff7947a79149 255 q1_ref = q1_ref_prev + q1_v*TS;
willem_hoitzing 12:ff7947a79149 256 q2_ref = q2_ref_prev + q2_v*TS;
willem_hoitzing 2:0a976fb06ff8 257 }
willem_hoitzing 2:0a976fb06ff8 258
willem_hoitzing 9:334b1596637b 259 void PID(float q1,float q1_ref,float q2,float q2_ref,float TS,float &motorValue1Out, float &motorValue2Out)
willem_hoitzing 9:334b1596637b 260 {
willem_hoitzing 9:334b1596637b 261 // linear feedback control
willem_hoitzing 10:f60f9849980a 262 q1_error = q1_ref - q1; //referencePosition1 - Position1; // proportional angular error in radians
willem_hoitzing 10:f60f9849980a 263 q2_error = q2_ref - q2; //referencePosition1 - Position1; // proportional angular error in radians
willem_hoitzing 10:f60f9849980a 264 float Kp = 10;
willem_hoitzing 5:0251fde34cdc 265
willem_hoitzing 10:f60f9849980a 266 q1IntError = q1IntError + q1_error*TS; // integrated error in radians
willem_hoitzing 10:f60f9849980a 267 q2IntError = q2IntError + q2_error*TS; // integrated error in radians
willem_hoitzing 11:03d979f1517f 268 float Ki = 1;
willem_hoitzing 10:f60f9849980a 269
willem_hoitzing 10:f60f9849980a 270 q1DerivativeError = (q1_error - q1_error_prev)/TS; // derivative of error in radians
willem_hoitzing 10:f60f9849980a 271 q2DerivativeError = (q2_error - q2_error_prev)/TS; // derivative of error in radians
willem_hoitzing 11:03d979f1517f 272 float Kd = 0;
willem_hoitzing 10:f60f9849980a 273
willem_hoitzing 12:ff7947a79149 274 q1_total_error = (q1_error * Kp) + (q1IntError * Ki) + (q1DerivativeError * Kd); //total controller output = motor input
willem_hoitzing 12:ff7947a79149 275 q2_total_error = (q2_error * Kp) + (q2IntError * Ki) + (q2DerivativeError * Kd); //total controller output = motor input
willem_hoitzing 10:f60f9849980a 276
willem_hoitzing 12:ff7947a79149 277 motorValue1Out = q1_total_error/MotorGain_M1;
willem_hoitzing 12:ff7947a79149 278 motorValue2Out = q2_total_error/MotorGain_M2;
willem_hoitzing 12:ff7947a79149 279
willem_hoitzing 9:334b1596637b 280 q1_error_prev = q1_error;
willem_hoitzing 9:334b1596637b 281 q2_error_prev = q2_error;
willem_hoitzing 5:0251fde34cdc 282 }
willem_hoitzing 5:0251fde34cdc 283
willem_hoitzing 9:334b1596637b 284 void Controller()
willem_hoitzing 5:0251fde34cdc 285 {
willem_hoitzing 9:334b1596637b 286 PID(q1,q1_ref,q2,q2_ref,TS,motorValue1Out,motorValue2Out);
willem_hoitzing 9:334b1596637b 287 ctrlOutput_M1 = motorValue1Out;
willem_hoitzing 9:334b1596637b 288 ctrlOutput_M2 = motorValue2Out;
willem_hoitzing 10:f60f9849980a 289
willem_hoitzing 12:ff7947a79149 290 pc.printf("q1_err = %0.9f \tq2_err = %0.9f \tq1IntErr = %0.9f \tq2IntErr = %0.9f \tq1_ref = %0.9f \tq2_ref = %0.9f \tq1 = %f \tq2 = %f\n\r",q1_error,q2_error,q1IntError,q2IntError,q1_ref,q2_ref,q1,q2);
willem_hoitzing 12:ff7947a79149 291
willem_hoitzing 5:0251fde34cdc 292 if (ctrlOutput_M1 < 0) {
willem_hoitzing 3:6ba52d1ae499 293 dir_M1 = 1;
willem_hoitzing 5:0251fde34cdc 294 } else {
willem_hoitzing 2:0a976fb06ff8 295 dir_M1 = 0;
willem_hoitzing 2:0a976fb06ff8 296 }
willem_hoitzing 6:4d254faf2428 297 pwm_M1 = abs(ctrlOutput_M1);
willem_hoitzing 9:334b1596637b 298 if (pwm_M1 <= 0) {
willem_hoitzing 8:008a7bf80fa0 299 pwm_M1 = 0;
willem_hoitzing 9:334b1596637b 300 } else {
willem_hoitzing 9:334b1596637b 301 pwm_M1 = pwm_M1 + 0.05;
willem_hoitzing 8:008a7bf80fa0 302 }
willem_hoitzing 5:0251fde34cdc 303
willem_hoitzing 5:0251fde34cdc 304 if (ctrlOutput_M2 < 0) {
willem_hoitzing 3:6ba52d1ae499 305 dir_M2 = 1;
willem_hoitzing 5:0251fde34cdc 306 } else {
willem_hoitzing 3:6ba52d1ae499 307 dir_M2 = 0;
willem_hoitzing 2:0a976fb06ff8 308 }
willem_hoitzing 6:4d254faf2428 309 pwm_M2 = abs(ctrlOutput_M2);
willem_hoitzing 9:334b1596637b 310 if (pwm_M2 <= 0) {
willem_hoitzing 8:008a7bf80fa0 311 pwm_M2 = 0;
willem_hoitzing 9:334b1596637b 312 } else {
willem_hoitzing 9:334b1596637b 313 pwm_M2 = pwm_M2 + 0.05;
willem_hoitzing 8:008a7bf80fa0 314 }
willem_hoitzing 0:26ce65a63616 315 }
willem_hoitzing 0:26ce65a63616 316
willem_hoitzing 0:26ce65a63616 317 int main()
willem_hoitzing 0:26ce65a63616 318 {
willem_hoitzing 11:03d979f1517f 319 ledr = 1;
willem_hoitzing 11:03d979f1517f 320 ledg = 1;
willem_hoitzing 11:03d979f1517f 321 ledb = 0;
willem_hoitzing 0:26ce65a63616 322 pc.baud(115200);
willem_hoitzing 5:0251fde34cdc 323 wheel_M1.reset();
willem_hoitzing 5:0251fde34cdc 324 wheel_M2.reset();
willem_hoitzing 7:1444604f10d4 325 knop_biceps.rise(&biceps);
willem_hoitzing 7:1444604f10d4 326 knop_triceps.rise(&triceps);
willem_hoitzing 7:1444604f10d4 327 knop_switch.rise(&switcher);
willem_hoitzing 12:ff7947a79149 328
willem_hoitzing 12:ff7947a79149 329 // initialize -> beginposities
willem_hoitzing 12:ff7947a79149 330 initialize();
willem_hoitzing 12:ff7947a79149 331
willem_hoitzing 5:0251fde34cdc 332 // flag functions/tickers
willem_hoitzing 7:1444604f10d4 333 update_encoder_ticker.attach(&flag_update_encoder, TS);
willem_hoitzing 7:1444604f10d4 334 update_ref_ticker.attach(&flag_update_ref, TS);
willem_hoitzing 10:f60f9849980a 335
willem_hoitzing 9:334b1596637b 336 if (active_PID_ticker == true) {
willem_hoitzing 10:f60f9849980a 337 PIDcontrol.attach(&flag_controller, TS);
willem_hoitzing 9:334b1596637b 338 }
willem_hoitzing 10:f60f9849980a 339
willem_hoitzing 5:0251fde34cdc 340 while(1) {
willem_hoitzing 10:f60f9849980a 341
willem_hoitzing 5:0251fde34cdc 342 // update encoder
willem_hoitzing 5:0251fde34cdc 343 if (go_flag_update_encoder == true) {
willem_hoitzing 5:0251fde34cdc 344 go_flag_update_encoder = false;
willem_hoitzing 5:0251fde34cdc 345 update_encoder();
willem_hoitzing 5:0251fde34cdc 346 }
willem_hoitzing 7:1444604f10d4 347 // update joint positions/velocities
willem_hoitzing 7:1444604f10d4 348 if (go_flag_update_ref == true) {
willem_hoitzing 7:1444604f10d4 349 go_flag_update_ref = false;
willem_hoitzing 7:1444604f10d4 350 update_ref();
willem_hoitzing 7:1444604f10d4 351 }
willem_hoitzing 9:334b1596637b 352 // controller M1+M2
willem_hoitzing 9:334b1596637b 353 if (go_flag_controller == true) {
willem_hoitzing 9:334b1596637b 354 go_flag_controller = false;
willem_hoitzing 9:334b1596637b 355 Controller();
willem_hoitzing 5:0251fde34cdc 356 }
willem_hoitzing 5:0251fde34cdc 357 }
willem_hoitzing 0:26ce65a63616 358 }