Wik de Haas / Mbed 2 deprecated Aansturing_knoppen

Aansturing

Dependencies:   Encoder mbed HIDScope

main.cpp@2:a42b34f9d6ab, 2016-11-02 (annotated)

Committer:
wikdehaas
Date:
Wed Nov 02 10:45:10 2016 +0000
Revision:
2:a42b34f9d6ab
Parent:
1:a644028231b5
Child:
3:57b98989b0b1
aansturing werkend (nu voor knoppen ingesteld);

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wikdehaas 0:55f9447aa02b 1 #include "mbed.h"
wikdehaas 0:55f9447aa02b 2 #include "PID.h"
wikdehaas 0:55f9447aa02b 3 #include "encoder.h"
wikdehaas 0:55f9447aa02b 4 #include "math.h"
wikdehaas 1:a644028231b5 5 #include "HIDScope.h"
wikdehaas 2:a42b34f9d6ab 6 #include "Filter.h"
wikdehaas 2:a42b34f9d6ab 7 #include "FilterDesign.h"
wikdehaas 2:a42b34f9d6ab 8 #include "Filter2.h"
wikdehaas 2:a42b34f9d6ab 9 #include "FilterDesign2.h"
wikdehaas 0:55f9447aa02b 10
wikdehaas 0:55f9447aa02b 11 //D8 doet het niet
wikdehaas 0:55f9447aa02b 12
wikdehaas 0:55f9447aa02b 13 DigitalIn knop_1(D2); //Motor 1
wikdehaas 0:55f9447aa02b 14 DigitalIn knop_2(D3); //Motor 2
wikdehaas 0:55f9447aa02b 15
wikdehaas 2:a42b34f9d6ab 16 AnalogIn emg1(A0);
wikdehaas 2:a42b34f9d6ab 17 AnalogIn emg2(A1);
wikdehaas 2:a42b34f9d6ab 18
wikdehaas 0:55f9447aa02b 19 DigitalOut motor_1(D7); //richting (1 of 0)
wikdehaas 0:55f9447aa02b 20 PwmOut pwm_motor_1(D6); //snelheid (tussen 0 en 1)
wikdehaas 0:55f9447aa02b 21 DigitalOut motor_2(D4); //richting (1 of 0)
wikdehaas 0:55f9447aa02b 22 PwmOut pwm_motor_2(D5); //snelheid (tussen 0 en 1)
wikdehaas 0:55f9447aa02b 23 InterruptIn stop(SW3); //stoppen
wikdehaas 2:a42b34f9d6ab 24 InterruptIn beginopnieuw(SW2); //stoppen
wikdehaas 0:55f9447aa02b 25
wikdehaas 0:55f9447aa02b 26 Serial pc(USBTX, USBRX); //USB ports aanroepen, D0 en D1
wikdehaas 0:55f9447aa02b 27
wikdehaas 0:55f9447aa02b 28 Encoder encoder_1(D13,D12);
wikdehaas 0:55f9447aa02b 29 Encoder encoder_2(D10,D9);
wikdehaas 0:55f9447aa02b 30
wikdehaas 1:a644028231b5 31 HIDScope scope(4);
wikdehaas 1:a644028231b5 32
wikdehaas 0:55f9447aa02b 33 Ticker PID_ticker;
wikdehaas 2:a42b34f9d6ab 34 Ticker FILTER_ticker;
wikdehaas 0:55f9447aa02b 35
wikdehaas 0:55f9447aa02b 36 Timer tijd;
wikdehaas 0:55f9447aa02b 37
wikdehaas 0:55f9447aa02b 38 //constante waardes
wikdehaas 2:a42b34f9d6ab 39 const double x_start = 0.255;
wikdehaas 0:55f9447aa02b 40 const double dt = 0.001;
wikdehaas 2:a42b34f9d6ab 41 const double dt_f = 0.001;
wikdehaas 0:55f9447aa02b 42 const double Vx = 0.05; //m/s
wikdehaas 1:a644028231b5 43 const double Vy = 0.03; //m/s
wikdehaas 2:a42b34f9d6ab 44 const double L2 = 0.35; //m
wikdehaas 2:a42b34f9d6ab 45 const double y_base = 0.045;
wikdehaas 0:55f9447aa02b 46 //filter
wikdehaas 0:55f9447aa02b 47 volatile double tijd_oud, tijd_nieuw, tijd_verschil;
wikdehaas 0:55f9447aa02b 48 volatile double u_1, y_1, u_2, y_2; //ongefilerd signaal emg 1, gefilterd signaal emg 1, ongefilterd signaal emg 2, gefilterd signaal emg 2
wikdehaas 0:55f9447aa02b 49 volatile int mode; //mode waarin de motors moeten bewegen
wikdehaas 0:55f9447aa02b 50 //controller
wikdehaas 0:55f9447aa02b 51 volatile double theta_1, theta_2, reference_1, plaats_1, error_1, pwm_1, reference_2, plaats_2, error_2, pwm_2;
wikdehaas 0:55f9447aa02b 52 //beginpositie
wikdehaas 0:55f9447aa02b 53 volatile double x = x_start; //m
wikdehaas 0:55f9447aa02b 54 volatile double y = y_base; //m
wikdehaas 0:55f9447aa02b 55 volatile bool opgepakt = false;
wikdehaas 0:55f9447aa02b 56 volatile bool zakpunt = false;
wikdehaas 0:55f9447aa02b 57 volatile bool uitdrukken = false;
wikdehaas 2:a42b34f9d6ab 58 volatile double y_oppakken = 0.10, y_stijgen = 0.15, x_zakken = 0.09;//Voorwaarde voor terugbewegen
wikdehaas 2:a42b34f9d6ab 59 volatile bool run_programma = true;
wikdehaas 0:55f9447aa02b 60 //PID
wikdehaas 2:a42b34f9d6ab 61 const double m1_Kp = 2, m1_Ki = 5, m1_Kd = 0.1, m1_N = 50;
wikdehaas 0:55f9447aa02b 62 double m1_v1 = 0, m1_v2 = 0; // Memory variables
wikdehaas 2:a42b34f9d6ab 63 const double m1_Ts = 0.001; // Controller sample time
wikdehaas 2:a42b34f9d6ab 64 const double m2_Kp = 2, m2_Ki = 5, m2_Kd = 0.1, m2_N = 50;
wikdehaas 0:55f9447aa02b 65 double m2_v1 = 0, m2_v2 = 0; // Memory variables
wikdehaas 2:a42b34f9d6ab 66 const double m2_Ts = 0.001; // Controller sample time
wikdehaas 0:55f9447aa02b 67
wikdehaas 0:55f9447aa02b 68
wikdehaas 0:55f9447aa02b 69 //Controller PID motors
wikdehaas 0:55f9447aa02b 70 void Controller()
wikdehaas 0:55f9447aa02b 71 {
wikdehaas 2:a42b34f9d6ab 72 theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
wikdehaas 0:55f9447aa02b 73 theta_2 = acos((x/L2)-cos(theta_1));
wikdehaas 0:55f9447aa02b 74
wikdehaas 1:a644028231b5 75 reference_1 = -1.5*theta_1; //reference
wikdehaas 0:55f9447aa02b 76 plaats_1 = 0.0014959*encoder_1.getPosition(); //positie encodercounts naar hoek
wikdehaas 0:55f9447aa02b 77 error_1 = reference_1 - plaats_1 ;
wikdehaas 0:55f9447aa02b 78 pwm_1 = PID(error_1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_N, m1_v1, m1_v2 );
wikdehaas 0:55f9447aa02b 79 if (pwm_1<0){motor_1 = 1;}
wikdehaas 0:55f9447aa02b 80 else {motor_1 = 0;}
wikdehaas 0:55f9447aa02b 81 pwm_motor_1 = fabs(pwm_1);
wikdehaas 0:55f9447aa02b 82
wikdehaas 1:a644028231b5 83 reference_2 = -1.5*theta_2; //reference
wikdehaas 0:55f9447aa02b 84 plaats_2 = 0.0014959*encoder_2.getPosition(); //positie encodercounts naar hoek
wikdehaas 0:55f9447aa02b 85 error_2 = reference_2 - plaats_2 ;
wikdehaas 0:55f9447aa02b 86 pwm_2 = PID(error_2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_N, m2_v1, m2_v2 );
wikdehaas 0:55f9447aa02b 87 if (pwm_2<0){motor_2 = 0;}
wikdehaas 0:55f9447aa02b 88 else {motor_2 = 1;}
wikdehaas 0:55f9447aa02b 89 pwm_motor_2 = fabs(pwm_2);
wikdehaas 2:a42b34f9d6ab 90 scope.set(0,error_1);
wikdehaas 2:a42b34f9d6ab 91 scope.set(1,reference_1);
wikdehaas 2:a42b34f9d6ab 92 scope.set(2,pwm_1);
wikdehaas 2:a42b34f9d6ab 93 scope.set(3,plaats_1);
wikdehaas 1:a644028231b5 94 scope.send();
wikdehaas 0:55f9447aa02b 95 }
wikdehaas 2:a42b34f9d6ab 96 //Ticker filterwaardes
wikdehaas 2:a42b34f9d6ab 97 void Filter_Samples()
wikdehaas 2:a42b34f9d6ab 98 {
wikdehaas 2:a42b34f9d6ab 99 u_1 = emg1.read();
wikdehaas 2:a42b34f9d6ab 100 y_1 = FilterDesign(u_1);
wikdehaas 2:a42b34f9d6ab 101 u_2 = emg2.read();
wikdehaas 2:a42b34f9d6ab 102 y_2 = FilterDesign2(u_2);
wikdehaas 2:a42b34f9d6ab 103 }
wikdehaas 0:55f9447aa02b 104
wikdehaas 0:55f9447aa02b 105 //Failsave
wikdehaas 0:55f9447aa02b 106 void Stop() //Zet allebei de snelheden op nul
wikdehaas 0:55f9447aa02b 107 {
wikdehaas 2:a42b34f9d6ab 108 PID_ticker.detach();
wikdehaas 0:55f9447aa02b 109 pwm_motor_1 = 0;
wikdehaas 0:55f9447aa02b 110 pwm_motor_2 = 0;
wikdehaas 2:a42b34f9d6ab 111 run_programma = false;
wikdehaas 2:a42b34f9d6ab 112 }
wikdehaas 2:a42b34f9d6ab 113
wikdehaas 2:a42b34f9d6ab 114 void Beginopnieuw()
wikdehaas 2:a42b34f9d6ab 115 {
wikdehaas 2:a42b34f9d6ab 116 run_programma = true;
wikdehaas 2:a42b34f9d6ab 117 x = x_start; y = y_base; m1_v1 = 0; m1_v2 = 0; m2_v1 = 0; m2_v2 = 0;
wikdehaas 2:a42b34f9d6ab 118 theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
wikdehaas 2:a42b34f9d6ab 119 theta_2 = acos((x/L2)-cos(theta_1));
wikdehaas 2:a42b34f9d6ab 120 double positie_motor_1 = -1.5*theta_1;
wikdehaas 2:a42b34f9d6ab 121 double positie_motor_2 = -1.5*theta_2;
wikdehaas 2:a42b34f9d6ab 122 encoder_1.setPosition(positie_motor_1/0.0014959); //positie encodercounts naar hoek
wikdehaas 2:a42b34f9d6ab 123 encoder_2.setPosition(positie_motor_2/0.0014959); //positie encodercounts naar hoek
wikdehaas 2:a42b34f9d6ab 124 PID_ticker.attach(&Controller,dt);
wikdehaas 2:a42b34f9d6ab 125 tijd.reset();
wikdehaas 2:a42b34f9d6ab 126 tijd.start();
wikdehaas 0:55f9447aa02b 127 }
wikdehaas 0:55f9447aa02b 128
wikdehaas 0:55f9447aa02b 129 int main()
wikdehaas 0:55f9447aa02b 130 {
wikdehaas 0:55f9447aa02b 131 pc.baud(115200);
wikdehaas 0:55f9447aa02b 132 stop.fall(&Stop); //stop de snelheden van de motoren bij indrukken
wikdehaas 2:a42b34f9d6ab 133 beginopnieuw.fall(&Beginopnieuw);
wikdehaas 2:a42b34f9d6ab 134 pwm_motor_1.period_us(1);
wikdehaas 2:a42b34f9d6ab 135 pwm_motor_2.period_us(1);
wikdehaas 2:a42b34f9d6ab 136 theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
wikdehaas 0:55f9447aa02b 137 theta_2 = acos((x/L2)-cos(theta_1));
wikdehaas 1:a644028231b5 138 double positie_motor_1 = -1.5*theta_1;
wikdehaas 1:a644028231b5 139 double positie_motor_2 = -1.5*theta_2;
wikdehaas 1:a644028231b5 140 encoder_1.setPosition(positie_motor_1/0.0014959); //positie encodercounts naar hoek
wikdehaas 1:a644028231b5 141 encoder_2.setPosition(positie_motor_2/0.0014959); //positie encodercounts naar hoek
wikdehaas 2:a42b34f9d6ab 142 FILTER_ticker.attach(&Filter_Samples,dt_f);
wikdehaas 1:a644028231b5 143 PID_ticker.attach(&Controller,dt);
wikdehaas 2:a42b34f9d6ab 144 wait(1);
wikdehaas 2:a42b34f9d6ab 145 tijd.reset();
wikdehaas 2:a42b34f9d6ab 146 tijd.start();
wikdehaas 0:55f9447aa02b 147 while(true)
wikdehaas 0:55f9447aa02b 148 {
wikdehaas 2:a42b34f9d6ab 149 while(run_programma)
wikdehaas 2:a42b34f9d6ab 150 {
wikdehaas 0:55f9447aa02b 151 tijd_nieuw = tijd;
wikdehaas 0:55f9447aa02b 152 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 0:55f9447aa02b 153 tijd_oud = tijd_nieuw;
wikdehaas 0:55f9447aa02b 154
wikdehaas 0:55f9447aa02b 155 if (knop_1 == 0 && knop_2 == 1){mode = 1;}
wikdehaas 0:55f9447aa02b 156 else if (knop_1 == 1 && knop_2 == 0){mode = 2;}
wikdehaas 2:a42b34f9d6ab 157 else if (knop_1 == 0 && knop_2 == 0 && x>(x_start+x_zakken)){mode = 3;}
wikdehaas 2:a42b34f9d6ab 158 else {mode = 4;} //default
wikdehaas 0:55f9447aa02b 159
wikdehaas 2:a42b34f9d6ab 160 /*
wikdehaas 2:a42b34f9d6ab 161 if (y_1 >= 0.4 && y_2 < 0.4) {mode = 1;}
wikdehaas 2:a42b34f9d6ab 162 else if (y_1 < 0.4 && y_2 >= 0.4) {mode = 2;}
wikdehaas 2:a42b34f9d6ab 163 else if (y_1 >= 0.4 && y_2 >= 0.4 && x>(x_start+x_zakken)){mode = 3;}
wikdehaas 2:a42b34f9d6ab 164 else {mode = 4;} //default
wikdehaas 2:a42b34f9d6ab 165 */
wikdehaas 0:55f9447aa02b 166 switch (mode)
wikdehaas 0:55f9447aa02b 167 {
wikdehaas 1:a644028231b5 168 case 1: x = x + tijd_verschil*Vx;
wikdehaas 1:a644028231b5 169 if (x>0.6){x = 0.6;}
wikdehaas 0:55f9447aa02b 170 y = y_base;
wikdehaas 0:55f9447aa02b 171 break;
wikdehaas 1:a644028231b5 172 case 2: x = x - tijd_verschil*Vx;
wikdehaas 1:a644028231b5 173 if (x<x_start){x = x_start;}
wikdehaas 0:55f9447aa02b 174 y = y_base;
wikdehaas 0:55f9447aa02b 175 break;
wikdehaas 0:55f9447aa02b 176 case 3: while(y > (y_base-y_oppakken))
wikdehaas 0:55f9447aa02b 177 {
wikdehaas 0:55f9447aa02b 178 tijd_nieuw = tijd;
wikdehaas 0:55f9447aa02b 179 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 2:a42b34f9d6ab 180 y = y - tijd_verschil*Vy*3;
wikdehaas 0:55f9447aa02b 181 x = x;
wikdehaas 0:55f9447aa02b 182 tijd_oud = tijd_nieuw;
wikdehaas 0:55f9447aa02b 183 }
wikdehaas 2:a42b34f9d6ab 184 wait(1);
wikdehaas 2:a42b34f9d6ab 185 tijd_nieuw = tijd;
wikdehaas 2:a42b34f9d6ab 186 tijd_oud = tijd_nieuw;
wikdehaas 0:55f9447aa02b 187 while(y < (y_base+y_stijgen))
wikdehaas 0:55f9447aa02b 188 {
wikdehaas 0:55f9447aa02b 189 tijd_nieuw = tijd;
wikdehaas 0:55f9447aa02b 190 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 2:a42b34f9d6ab 191 y = y + tijd_verschil*Vy*5;
wikdehaas 0:55f9447aa02b 192 x = x;
wikdehaas 0:55f9447aa02b 193 tijd_oud = tijd_nieuw;
wikdehaas 0:55f9447aa02b 194 }
wikdehaas 0:55f9447aa02b 195 while(x > (x_start+x_zakken))
wikdehaas 0:55f9447aa02b 196 {
wikdehaas 0:55f9447aa02b 197 tijd_nieuw = tijd;
wikdehaas 0:55f9447aa02b 198 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 2:a42b34f9d6ab 199 x = x-tijd_verschil*Vx*2;
wikdehaas 0:55f9447aa02b 200 tijd_oud = tijd_nieuw;
wikdehaas 0:55f9447aa02b 201 }
wikdehaas 0:55f9447aa02b 202 while(y>y_base)
wikdehaas 0:55f9447aa02b 203 {
wikdehaas 0:55f9447aa02b 204 tijd_nieuw = tijd;
wikdehaas 0:55f9447aa02b 205 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 2:a42b34f9d6ab 206 y = y - tijd_verschil*Vy*2;
wikdehaas 0:55f9447aa02b 207 x = x;
wikdehaas 0:55f9447aa02b 208 tijd_oud = tijd_nieuw;
wikdehaas 0:55f9447aa02b 209 }
wikdehaas 2:a42b34f9d6ab 210 while(x>x_start-0.02)
wikdehaas 0:55f9447aa02b 211 {
wikdehaas 0:55f9447aa02b 212 tijd_nieuw = tijd;
wikdehaas 0:55f9447aa02b 213 tijd_verschil = tijd_nieuw - tijd_oud;
wikdehaas 0:55f9447aa02b 214 y = y_base;
wikdehaas 2:a42b34f9d6ab 215 x = x - tijd_verschil*Vx*2;
wikdehaas 0:55f9447aa02b 216 tijd_oud = tijd_nieuw;
wikdehaas 0:55f9447aa02b 217 }
wikdehaas 1:a644028231b5 218 wait(0.5);
wikdehaas 0:55f9447aa02b 219 x = x_start;
wikdehaas 0:55f9447aa02b 220 y = y_base;
wikdehaas 0:55f9447aa02b 221 break;
wikdehaas 0:55f9447aa02b 222 default: x = x;
wikdehaas 2:a42b34f9d6ab 223 y = y;
wikdehaas 0:55f9447aa02b 224 break;
wikdehaas 0:55f9447aa02b 225 }
wikdehaas 0:55f9447aa02b 226 }
wikdehaas 2:a42b34f9d6ab 227 }
wikdehaas 0:55f9447aa02b 228 }