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@7:dd3cba61b34b, 2014-10-17 (annotated)

Committer:
JKleinRot
Date:
Fri Oct 17 07:52:22 2014 +0000
Revision:
7:dd3cba61b34b
Parent:
6:3b6fad529520
Child:
8:d4161e9be1da
Child:
9:454e7da8ab65
2014-10-17. For loop werkt, ; ipv ,. Arm naar positie bij de kalibratie in voids gezet voor de main loop, zo overzichtelijker en minder tekst in main loop

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 7:dd3cba61b34b 37 int puls_arm1_home = 363; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 1 op de thuispositie is
JKleinRot 7:dd3cba61b34b 38 int puls_arm2_home = 787; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 2 op de thuispositie is
JKleinRot 7:dd3cba61b34b 39 float pwm_to_motor_arm1; //PWM output naar motor arm 1
JKleinRot 7:dd3cba61b34b 40 float pwm_to_motor_arm2; //PWM output naar motor arm 2
JKleinRot 7:dd3cba61b34b 41 float xbk; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 7:dd3cba61b34b 42 int i; //
JKleinRot 7:dd3cba61b34b 43
JKleinRot 6:3b6fad529520 44 volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 6:3b6fad529520 45 void setregelaar_ticker_flag(){ //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
JKleinRot 6:3b6fad529520 46 regelaar_ticker_flag = true;
JKleinRot 1:e5e1eb9d0025 47 }
JKleinRot 1:e5e1eb9d0025 48
JKleinRot 6:3b6fad529520 49 volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 6:3b6fad529520 50 void setregelaar_EMG_flag(){ //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
JKleinRot 6:3b6fad529520 51 regelaar_EMG_flag = true;
JKleinRot 4:69540b9c0646 52 }
JKleinRot 4:69540b9c0646 53
JKleinRot 6:3b6fad529520 54 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 55 if (*in < min){ //Als de waarde kleiner is als het minimum wordt de waarde het minimum
JKleinRot 2:0cb899f2800a 56 *in = min;
JKleinRot 2:0cb899f2800a 57 }
JKleinRot 6:3b6fad529520 58 if (*in > max){ //Als de waarde groter is dan het maximum is de waarde het maximum
JKleinRot 2:0cb899f2800a 59 *in = max;
JKleinRot 2:0cb899f2800a 60 }
JKleinRot 6:3b6fad529520 61 else { //In alle andere gevallen is de waarde de waarde zelf
JKleinRot 2:0cb899f2800a 62 *in = *in;
JKleinRot 2:0cb899f2800a 63 }
JKleinRot 2:0cb899f2800a 64 }
JKleinRot 1:e5e1eb9d0025 65
JKleinRot 7:dd3cba61b34b 66 void arm1_naar_thuispositie(){
JKleinRot 7:dd3cba61b34b 67 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 7:dd3cba61b34b 68 keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 7:dd3cba61b34b 69
JKleinRot 7:dd3cba61b34b 70 if (pwm_to_motor_arm1 > 0){ //Als PWM is positief, dan richting 1
JKleinRot 7:dd3cba61b34b 71 dir_motor_arm1.write(1);
JKleinRot 7:dd3cba61b34b 72 }
JKleinRot 7:dd3cba61b34b 73 else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 74 dir_motor_arm1.write(0);
JKleinRot 7:dd3cba61b34b 75 }
JKleinRot 7:dd3cba61b34b 76 pwm_motor_arm1.write(abs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 7:dd3cba61b34b 77
JKleinRot 7:dd3cba61b34b 78 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 7:dd3cba61b34b 79 kalibratie_positie_arm1 = true;
JKleinRot 7:dd3cba61b34b 80 pc.printf("Arm 1 naar thuispositie compleet"); //Tekst voor controle Arm 1 naar thuispositie
JKleinRot 7:dd3cba61b34b 81 }
JKleinRot 7:dd3cba61b34b 82 }
JKleinRot 7:dd3cba61b34b 83
JKleinRot 7:dd3cba61b34b 84 void arm2_naar_thuispositie(){
JKleinRot 7:dd3cba61b34b 85 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 7:dd3cba61b34b 86 keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 7:dd3cba61b34b 87
JKleinRot 7:dd3cba61b34b 88 if (pwm_to_motor_arm2 > 0){ //Als PWM is positief, dan richting 1
JKleinRot 7:dd3cba61b34b 89 dir_motor_arm2.write(1);
JKleinRot 7:dd3cba61b34b 90 }
JKleinRot 7:dd3cba61b34b 91 else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 92 dir_motor_arm2.write(0);
JKleinRot 7:dd3cba61b34b 93 }
JKleinRot 7:dd3cba61b34b 94 pwm_motor_arm2.write(abs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 7:dd3cba61b34b 95
JKleinRot 7:dd3cba61b34b 96 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 7:dd3cba61b34b 97 kalibratie_positie_arm2 = true;
JKleinRot 7:dd3cba61b34b 98 pc.printf("Arm 2 naar thuispositie compleet"); //Tekst voor controle Arm 2 naar thuispositie
JKleinRot 7:dd3cba61b34b 99 }
JKleinRot 7:dd3cba61b34b 100 }
JKleinRot 0:859c89785d3f 101
JKleinRot 0:859c89785d3f 102
JKleinRot 0:859c89785d3f 103 int main() {
JKleinRot 0:859c89785d3f 104
JKleinRot 0:859c89785d3f 105 //PC baud rate instellen
JKleinRot 0:859c89785d3f 106 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 0:859c89785d3f 107
JKleinRot 0:859c89785d3f 108 //Aanzetten
JKleinRot 4:69540b9c0646 109 if (rust == false && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 0:859c89785d3f 110 lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
JKleinRot 0:859c89785d3f 111 lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
JKleinRot 0:859c89785d3f 112 wait(2); //Twee seconden wachten
JKleinRot 0:859c89785d3f 113 pc.printf("Aanzetten compleet"); //Tekst voor controle Aanzetten
JKleinRot 0:859c89785d3f 114 rust = true; //Rust wordt waar zodat door kan worden gegaan naar het volgende deel
JKleinRot 0:859c89785d3f 115 }
JKleinRot 1:e5e1eb9d0025 116
JKleinRot 1:e5e1eb9d0025 117
JKleinRot 1:e5e1eb9d0025 118
JKleinRot 1:e5e1eb9d0025 119 //Arm 1 naar thuispositie
JKleinRot 4:69540b9c0646 120 if (rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 1:e5e1eb9d0025 121 wait(1); //Een seconde wachten
JKleinRot 1:e5e1eb9d0025 122
JKleinRot 1:e5e1eb9d0025 123 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 1:e5e1eb9d0025 124
JKleinRot 4:69540b9c0646 125 while(rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) {
JKleinRot 6:3b6fad529520 126 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 127 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 1:e5e1eb9d0025 128
JKleinRot 7:dd3cba61b34b 129 arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
JKleinRot 1:e5e1eb9d0025 130 }
JKleinRot 7:dd3cba61b34b 131 wait(1); //Een seconde wachten
JKleinRot 3:adc3052039e7 132 }
JKleinRot 3:adc3052039e7 133
JKleinRot 3:adc3052039e7 134 //Arm 2 naar thuispositie
JKleinRot 7:dd3cba61b34b 135 if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 3:adc3052039e7 136 wait(1); //Een seconde wachten
JKleinRot 3:adc3052039e7 137
JKleinRot 3:adc3052039e7 138 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 3:adc3052039e7 139
JKleinRot 4:69540b9c0646 140 while(rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) {
JKleinRot 6:3b6fad529520 141 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 142 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 3:adc3052039e7 143
JKleinRot 7:dd3cba61b34b 144 arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
JKleinRot 3:adc3052039e7 145 }
JKleinRot 6:3b6fad529520 146 wait(1); //Een seconde wachten
JKleinRot 6:3b6fad529520 147 ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
JKleinRot 3:adc3052039e7 148 }
JKleinRot 4:69540b9c0646 149
JKleinRot 4:69540b9c0646 150 //Ticker voor kalibratie
JKleinRot 4:69540b9c0646 151 if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == true && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 4:69540b9c0646 152
JKleinRot 6:3b6fad529520 153 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken
JKleinRot 6:3b6fad529520 154 pc.printf("Ticker voor kalibratie compleet"); //Tekst voor controle Ticker voor kalibratie
JKleinRot 5:a1dcd605dd3d 155
JKleinRot 5:a1dcd605dd3d 156 //5 seconden EMG biceps meten
JKleinRot 5:a1dcd605dd3d 157
JKleinRot 6:3b6fad529520 158 wait(1); //Een seconde wachten
JKleinRot 6:3b6fad529520 159 lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm
JKleinRot 6:3b6fad529520 160 lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm
JKleinRot 6:3b6fad529520 161 wait(2); //Twee seconden wachten
JKleinRot 5:a1dcd605dd3d 162
JKleinRot 7:dd3cba61b34b 163 for (i=0; i<1000; i++){
JKleinRot 7:dd3cba61b34b 164 while(regelaar_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 165 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 5:a1dcd605dd3d 166
JKleinRot 6:3b6fad529520 167 xbk = EMG_bi.read(); //EMG signaal uitlezen
JKleinRot 5:a1dcd605dd3d 168 }
JKleinRot 5:a1dcd605dd3d 169
JKleinRot 4:69540b9c0646 170 }
JKleinRot 4:69540b9c0646 171
JKleinRot 4:69540b9c0646 172
JKleinRot 4:69540b9c0646 173
JKleinRot 4:69540b9c0646 174
JKleinRot 0:859c89785d3f 175 }