Wik de Haas / Mbed 2 deprecated Aansturing_Groep_4

Aansturing robot groep 4 Biorobotics 2016. Beer bottle remover

Dependencies:   Encoder HIDScope mbed

Fork of Aansturing_knoppen by Wik de Haas

main.cpp@4:9df6a1735cef, 2016-11-09 (annotated)

Committer:
wikdehaas
Date:
Wed Nov 09 08:47:12 2016 +0000
Revision:
4:9df6a1735cef
Parent:
3:57b98989b0b1 
Program group 4. Beer bottle remover. Simplified for the cause of demonstration.

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