Janine Klein Rot / Mbed 2 deprecated PIPO-2

2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range

Dependencies:   Encoder MODSERIAL TextLCD mbed mbed-dsp

main.cpp@6:3b6fad529520, 2014-10-16 (annotated)

Committer:
JKleinRot
Date:
Thu Oct 16 15:09:46 2014 +0000
Revision:
6:3b6fad529520
Parent:
5:a1dcd605dd3d
Child:
7:dd3cba61b34b
2014-10-16 Revision 5 met commentaar. Ticker werkt nu, maar de for loop om het EMG meten 5 seconden te laten duren werkt niet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JKleinRot 0:859c89785d3f 1 #include "mbed.h" //Mbed bibliotheek inladen, standaard functies
JKleinRot 0:859c89785d3f 2 #include "MODSERIAL.h" //MODSERIAL bibliotheek inladen, communicatie met PC
JKleinRot 0:859c89785d3f 3 #include "encoder.h" //Encoder bibliotheek inladen, communicatie met encoder
JKleinRot 0:859c89785d3f 4 #include "TextLCD.h" //LCD scherm bibliotheek inladen, communicatie met LCD scherm
JKleinRot 0:859c89785d3f 5
JKleinRot 0:859c89785d3f 6 //Constanten definiëren en waarde geven
JKleinRot 6:3b6fad529520 7 #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
JKleinRot 6:3b6fad529520 8 #define KP 1 //Factor proprotionele regelaar
JKleinRot 6:3b6fad529520 9 #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
JKleinRot 0:859c89785d3f 10
JKleinRot 0:859c89785d3f 11 //Pinverdeling en naamgeving variabelen
JKleinRot 0:859c89785d3f 12 TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm
JKleinRot 0:859c89785d3f 13 MODSERIAL pc(USBTX, USBRX); //PC
JKleinRot 0:859c89785d3f 14
JKleinRot 1:e5e1eb9d0025 15 PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
JKleinRot 1:e5e1eb9d0025 16 DigitalOut dir_motor_arm1(PTD1); //Richting van motor arm 1
JKleinRot 1:e5e1eb9d0025 17 Encoder puls_motor_arm1(PTD0, PTC9); //Encoder pulsen tellen van motor arm 1
JKleinRot 6:3b6fad529520 18 PwmOut pwm_motor_arm2(PTA12); //PWM naar motor arm 2
JKleinRot 6:3b6fad529520 19 DigitalOut dir_motor_arm2(PTD3); //Ricting van motor arm 2
JKleinRot 6:3b6fad529520 20 Encoder puls_motor_arm2(PTD4, PTC8); //Encoder pulsen tellen van motor arm 2
JKleinRot 6:3b6fad529520 21 AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps
JKleinRot 1:e5e1eb9d0025 22
JKleinRot 4:69540b9c0646 23
JKleinRot 6:3b6fad529520 24 Ticker ticker_regelaar; //Ticker voor regelaar motor
JKleinRot 6:3b6fad529520 25 Ticker ticker_EMG; //Ticker voor EMG meten
JKleinRot 1:e5e1eb9d0025 26
JKleinRot 6:3b6fad529520 27 //Gedefinieerde datatypen en naamgeving en beginwaarden
JKleinRot 0:859c89785d3f 28 bool rust = false; //Bool voor controleren volgorde in programma
JKleinRot 3:adc3052039e7 29 bool kalibratie_positie_arm1 = false; //Bool voor controleren volgorde in programma
JKleinRot 6:3b6fad529520 30 bool kalibratie_positie_arm2 = false; //Bool voor controleren volgorde in programma
JKleinRot 6:3b6fad529520 31 bool kalibratie_EMG_bi = false; //Bool voor controleren volgorde in programma
JKleinRot 6:3b6fad529520 32 bool kalibratie_EMG_tri = false; //Bool voor controleren volgorde in programma
JKleinRot 0:859c89785d3f 33
JKleinRot 6:3b6fad529520 34 char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm
JKleinRot 6:3b6fad529520 35 char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm
JKleinRot 0:859c89785d3f 36
JKleinRot 6:3b6fad529520 37 volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 6:3b6fad529520 38 void setregelaar_ticker_flag(){ //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
JKleinRot 6:3b6fad529520 39 regelaar_ticker_flag = true;
JKleinRot 1:e5e1eb9d0025 40 }
JKleinRot 1:e5e1eb9d0025 41
JKleinRot 6:3b6fad529520 42 volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 6:3b6fad529520 43 void setregelaar_EMG_flag(){ //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
JKleinRot 6:3b6fad529520 44 regelaar_EMG_flag = true;
JKleinRot 4:69540b9c0646 45 }
JKleinRot 4:69540b9c0646 46
JKleinRot 6:3b6fad529520 47 void keep_in_range(float * in, float min, float max){ //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt
JKleinRot 6:3b6fad529520 48 if (*in < min){ //Als de waarde kleiner is als het minimum wordt de waarde het minimum
JKleinRot 2:0cb899f2800a 49 *in = min;
JKleinRot 2:0cb899f2800a 50 }
JKleinRot 6:3b6fad529520 51 if (*in > max){ //Als de waarde groter is dan het maximum is de waarde het maximum
JKleinRot 2:0cb899f2800a 52 *in = max;
JKleinRot 2:0cb899f2800a 53 }
JKleinRot 6:3b6fad529520 54 else { //In alle andere gevallen is de waarde de waarde zelf
JKleinRot 2:0cb899f2800a 55 *in = *in;
JKleinRot 2:0cb899f2800a 56 }
JKleinRot 2:0cb899f2800a 57 }
JKleinRot 2:0cb899f2800a 58
JKleinRot 1:e5e1eb9d0025 59
JKleinRot 6:3b6fad529520 60 int puls_arm1_home = 363; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 1 op de thuispositie is
JKleinRot 6:3b6fad529520 61 int puls_arm2_home = 787; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 2 op de thuispositie is
JKleinRot 6:3b6fad529520 62 float pwm_to_motor_arm1; //PWM output naar motor arm 1
JKleinRot 6:3b6fad529520 63 float pwm_to_motor_arm2; //PWM output naar motor arm 2
JKleinRot 6:3b6fad529520 64 float xbk; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 6:3b6fad529520 65 int i = 0; //
JKleinRot 0:859c89785d3f 66
JKleinRot 0:859c89785d3f 67
JKleinRot 0:859c89785d3f 68 int main() {
JKleinRot 0:859c89785d3f 69
JKleinRot 0:859c89785d3f 70 //PC baud rate instellen
JKleinRot 0:859c89785d3f 71 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 0:859c89785d3f 72
JKleinRot 0:859c89785d3f 73 //Aanzetten
JKleinRot 4:69540b9c0646 74 if (rust == false && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 0:859c89785d3f 75 lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
JKleinRot 0:859c89785d3f 76 lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
JKleinRot 0:859c89785d3f 77 wait(2); //Twee seconden wachten
JKleinRot 0:859c89785d3f 78 pc.printf("Aanzetten compleet"); //Tekst voor controle Aanzetten
JKleinRot 0:859c89785d3f 79 rust = true; //Rust wordt waar zodat door kan worden gegaan naar het volgende deel
JKleinRot 0:859c89785d3f 80 }
JKleinRot 1:e5e1eb9d0025 81
JKleinRot 1:e5e1eb9d0025 82
JKleinRot 1:e5e1eb9d0025 83
JKleinRot 1:e5e1eb9d0025 84 //Arm 1 naar thuispositie
JKleinRot 4:69540b9c0646 85 if (rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 1:e5e1eb9d0025 86 wait(1); //Een seconde wachten
JKleinRot 1:e5e1eb9d0025 87
JKleinRot 1:e5e1eb9d0025 88 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 1:e5e1eb9d0025 89
JKleinRot 4:69540b9c0646 90 while(rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) {
JKleinRot 6:3b6fad529520 91 while(regelaar_ticker_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 6:3b6fad529520 92 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 1:e5e1eb9d0025 93
JKleinRot 6:3b6fad529520 94 pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP; //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
JKleinRot 6:3b6fad529520 95 keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 1:e5e1eb9d0025 96
JKleinRot 6:3b6fad529520 97 if (pwm_to_motor_arm1 > 0){ //Als PWM is positief, dan richting 1
JKleinRot 1:e5e1eb9d0025 98 dir_motor_arm1.write(1);
JKleinRot 1:e5e1eb9d0025 99 }
JKleinRot 6:3b6fad529520 100 else { //Anders richting nul
JKleinRot 1:e5e1eb9d0025 101 dir_motor_arm1.write(0);
JKleinRot 1:e5e1eb9d0025 102 }
JKleinRot 6:3b6fad529520 103 pwm_motor_arm1.write(abs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 2:0cb899f2800a 104
JKleinRot 6:3b6fad529520 105 if (pwm_to_motor_arm1 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
JKleinRot 3:adc3052039e7 106 kalibratie_positie_arm1 = true;
JKleinRot 6:3b6fad529520 107 pc.printf("Arm 1 naar thuispositie compleet"); //Tekst voor controle Arm 1 naar thuispositie
JKleinRot 2:0cb899f2800a 108 }
JKleinRot 6:3b6fad529520 109 wait(1); //Een seconde wachten
JKleinRot 2:0cb899f2800a 110
JKleinRot 1:e5e1eb9d0025 111 }
JKleinRot 3:adc3052039e7 112 }
JKleinRot 3:adc3052039e7 113
JKleinRot 3:adc3052039e7 114 //Arm 2 naar thuispositie
JKleinRot 4:69540b9c0646 115 if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 3:adc3052039e7 116 wait(1); //Een seconde wachten
JKleinRot 3:adc3052039e7 117
JKleinRot 3:adc3052039e7 118 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 3:adc3052039e7 119
JKleinRot 4:69540b9c0646 120 while(rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) {
JKleinRot 6:3b6fad529520 121 while(regelaar_ticker_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 6:3b6fad529520 122 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 3:adc3052039e7 123
JKleinRot 6:3b6fad529520 124 pwm_to_motor_arm2 = (puls_arm2_home - puls_motor_arm2.getPosition())*KP; //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
JKleinRot 6:3b6fad529520 125 keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 3:adc3052039e7 126
JKleinRot 6:3b6fad529520 127 if (pwm_to_motor_arm2 > 0){ //Als PWM is positief, dan richting 1
JKleinRot 3:adc3052039e7 128 dir_motor_arm2.write(1);
JKleinRot 3:adc3052039e7 129 }
JKleinRot 6:3b6fad529520 130 else { //Anders richting nul
JKleinRot 3:adc3052039e7 131 dir_motor_arm2.write(0);
JKleinRot 3:adc3052039e7 132 }
JKleinRot 6:3b6fad529520 133 pwm_motor_arm2.write(abs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 3:adc3052039e7 134
JKleinRot 6:3b6fad529520 135 if (pwm_to_motor_arm2 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
JKleinRot 3:adc3052039e7 136 kalibratie_positie_arm2 = true;
JKleinRot 6:3b6fad529520 137 pc.printf("Arm 2 naar thuispositie compleet"); //Tekst voor controle Arm 2 naar thuispositie
JKleinRot 3:adc3052039e7 138 }
JKleinRot 3:adc3052039e7 139 }
JKleinRot 6:3b6fad529520 140 wait(1); //Een seconde wachten
JKleinRot 6:3b6fad529520 141 ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
JKleinRot 3:adc3052039e7 142 }
JKleinRot 4:69540b9c0646 143
JKleinRot 4:69540b9c0646 144 //Ticker voor kalibratie
JKleinRot 4:69540b9c0646 145 if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == true && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 4:69540b9c0646 146
JKleinRot 6:3b6fad529520 147 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken
JKleinRot 6:3b6fad529520 148 pc.printf("Ticker voor kalibratie compleet"); //Tekst voor controle Ticker voor kalibratie
JKleinRot 5:a1dcd605dd3d 149
JKleinRot 5:a1dcd605dd3d 150 //5 seconden EMG biceps meten
JKleinRot 5:a1dcd605dd3d 151
JKleinRot 6:3b6fad529520 152 wait(1); //Een seconde wachten
JKleinRot 6:3b6fad529520 153 lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm
JKleinRot 6:3b6fad529520 154 lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm
JKleinRot 6:3b6fad529520 155 wait(2); //Twee seconden wachten
JKleinRot 5:a1dcd605dd3d 156
JKleinRot 5:a1dcd605dd3d 157 for (i=0 ,i<5000, i++){
JKleinRot 6:3b6fad529520 158 while(setregelaar_EMG_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 6:3b6fad529520 159 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 5:a1dcd605dd3d 160
JKleinRot 6:3b6fad529520 161 xbk = EMG_bi.read(); //EMG signaal uitlezen
JKleinRot 5:a1dcd605dd3d 162 }
JKleinRot 5:a1dcd605dd3d 163
JKleinRot 4:69540b9c0646 164 }
JKleinRot 4:69540b9c0646 165
JKleinRot 4:69540b9c0646 166
JKleinRot 4:69540b9c0646 167
JKleinRot 4:69540b9c0646 168
JKleinRot 0:859c89785d3f 169 }