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@8:d4161e9be1da, 2014-10-20 (annotated)

Committer:
JKleinRot
Date:
Mon Oct 20 11:56:38 2014 +0000
Revision:
8:d4161e9be1da
Parent:
7:dd3cba61b34b 
2014-10-20. Nu met motor gedaan. Arm 1 naar thuispositie werkt niet. Pinindeling van K en T bepalen zorgt voor lopen andere motor

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 8:d4161e9be1da 8 #define KP 0.001 //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 8:d4161e9be1da 12 TextLCD lcd(PTD3, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm
JKleinRot 0:859c89785d3f 13 MODSERIAL pc(USBTX, USBRX); //PC
JKleinRot 0:859c89785d3f 14
JKleinRot 8:d4161e9be1da 15 PwmOut pwm_motor_arm1(PTC8); //PWM naar motor arm 1
JKleinRot 8:d4161e9be1da 16 DigitalOut dir_motor_arm1(PTC9); //Richting van motor arm 1
JKleinRot 8:d4161e9be1da 17 Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1
JKleinRot 8:d4161e9be1da 18 PwmOut pwm_motor_arm2(PTA5); //PWM naar motor arm 2
JKleinRot 8:d4161e9be1da 19 DigitalOut dir_motor_arm2(PTA4); //Ricting van motor arm 2
JKleinRot 8:d4161e9be1da 20 Encoder puls_motor_arm2(PTD5, PTA13); //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 8:d4161e9be1da 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 8:d4161e9be1da 77 pc.printf("pwm_to_motor_arm1 is %d\n\r",pwm_to_motor_arm1);
JKleinRot 7:dd3cba61b34b 78
JKleinRot 8:d4161e9be1da 79 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 80 kalibratie_positie_arm1 = true;
JKleinRot 7:dd3cba61b34b 81 pc.printf("Arm 1 naar thuispositie compleet"); //Tekst voor controle Arm 1 naar thuispositie
JKleinRot 7:dd3cba61b34b 82 }
JKleinRot 7:dd3cba61b34b 83 }
JKleinRot 7:dd3cba61b34b 84
JKleinRot 7:dd3cba61b34b 85 void arm2_naar_thuispositie(){
JKleinRot 7:dd3cba61b34b 86 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 87 keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 7:dd3cba61b34b 88
JKleinRot 7:dd3cba61b34b 89 if (pwm_to_motor_arm2 > 0){ //Als PWM is positief, dan richting 1
JKleinRot 7:dd3cba61b34b 90 dir_motor_arm2.write(1);
JKleinRot 7:dd3cba61b34b 91 }
JKleinRot 7:dd3cba61b34b 92 else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 93 dir_motor_arm2.write(0);
JKleinRot 7:dd3cba61b34b 94 }
JKleinRot 7:dd3cba61b34b 95 pwm_motor_arm2.write(abs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 8:d4161e9be1da 96 pc.printf("pwm_to_motor_arm2 is %f\n\r",puls_motor_arm2.getPosition());
JKleinRot 7:dd3cba61b34b 97
JKleinRot 7:dd3cba61b34b 98 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 99 kalibratie_positie_arm2 = true;
JKleinRot 7:dd3cba61b34b 100 pc.printf("Arm 2 naar thuispositie compleet"); //Tekst voor controle Arm 2 naar thuispositie
JKleinRot 7:dd3cba61b34b 101 }
JKleinRot 7:dd3cba61b34b 102 }
JKleinRot 0:859c89785d3f 103
JKleinRot 0:859c89785d3f 104
JKleinRot 0:859c89785d3f 105 int main() {
JKleinRot 0:859c89785d3f 106
JKleinRot 0:859c89785d3f 107 //PC baud rate instellen
JKleinRot 0:859c89785d3f 108 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 0:859c89785d3f 109
JKleinRot 0:859c89785d3f 110 //Aanzetten
JKleinRot 4:69540b9c0646 111 if (rust == false && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 0:859c89785d3f 112 lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
JKleinRot 0:859c89785d3f 113 lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
JKleinRot 0:859c89785d3f 114 wait(2); //Twee seconden wachten
JKleinRot 0:859c89785d3f 115 pc.printf("Aanzetten compleet"); //Tekst voor controle Aanzetten
JKleinRot 0:859c89785d3f 116 rust = true; //Rust wordt waar zodat door kan worden gegaan naar het volgende deel
JKleinRot 0:859c89785d3f 117 }
JKleinRot 1:e5e1eb9d0025 118
JKleinRot 1:e5e1eb9d0025 119
JKleinRot 1:e5e1eb9d0025 120
JKleinRot 1:e5e1eb9d0025 121 //Arm 1 naar thuispositie
JKleinRot 4:69540b9c0646 122 if (rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 1:e5e1eb9d0025 123 wait(1); //Een seconde wachten
JKleinRot 1:e5e1eb9d0025 124
JKleinRot 1:e5e1eb9d0025 125 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 1:e5e1eb9d0025 126
JKleinRot 4:69540b9c0646 127 while(rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) {
JKleinRot 6:3b6fad529520 128 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 129 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 1:e5e1eb9d0025 130
JKleinRot 7:dd3cba61b34b 131 arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
JKleinRot 1:e5e1eb9d0025 132 }
JKleinRot 7:dd3cba61b34b 133 wait(1); //Een seconde wachten
JKleinRot 3:adc3052039e7 134 }
JKleinRot 8:d4161e9be1da 135 pc.printf("hoi");
JKleinRot 3:adc3052039e7 136 //Arm 2 naar thuispositie
JKleinRot 7:dd3cba61b34b 137 if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 3:adc3052039e7 138 wait(1); //Een seconde wachten
JKleinRot 3:adc3052039e7 139
JKleinRot 3:adc3052039e7 140 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 3:adc3052039e7 141
JKleinRot 4:69540b9c0646 142 while(rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) {
JKleinRot 6:3b6fad529520 143 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 144 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 3:adc3052039e7 145
JKleinRot 7:dd3cba61b34b 146 arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
JKleinRot 3:adc3052039e7 147 }
JKleinRot 6:3b6fad529520 148 wait(1); //Een seconde wachten
JKleinRot 6:3b6fad529520 149 ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
JKleinRot 3:adc3052039e7 150 }
JKleinRot 4:69540b9c0646 151
JKleinRot 4:69540b9c0646 152 //Ticker voor kalibratie
JKleinRot 4:69540b9c0646 153 if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == true && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 4:69540b9c0646 154
JKleinRot 6:3b6fad529520 155 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken
JKleinRot 6:3b6fad529520 156 pc.printf("Ticker voor kalibratie compleet"); //Tekst voor controle Ticker voor kalibratie
JKleinRot 5:a1dcd605dd3d 157
JKleinRot 5:a1dcd605dd3d 158 //5 seconden EMG biceps meten
JKleinRot 5:a1dcd605dd3d 159
JKleinRot 6:3b6fad529520 160 wait(1); //Een seconde wachten
JKleinRot 6:3b6fad529520 161 lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm
JKleinRot 6:3b6fad529520 162 lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm
JKleinRot 6:3b6fad529520 163 wait(2); //Twee seconden wachten
JKleinRot 5:a1dcd605dd3d 164
JKleinRot 7:dd3cba61b34b 165 for (i=0; i<1000; i++){
JKleinRot 7:dd3cba61b34b 166 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 167 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 5:a1dcd605dd3d 168
JKleinRot 6:3b6fad529520 169 xbk = EMG_bi.read(); //EMG signaal uitlezen
JKleinRot 5:a1dcd605dd3d 170 }
JKleinRot 5:a1dcd605dd3d 171
JKleinRot 4:69540b9c0646 172 }
JKleinRot 4:69540b9c0646 173
JKleinRot 4:69540b9c0646 174
JKleinRot 4:69540b9c0646 175
JKleinRot 4:69540b9c0646 176
JKleinRot 0:859c89785d3f 177 }