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@9:454e7da8ab65, 2014-10-20 (annotated)

Committer:
JKleinRot
Date:
Mon Oct 20 20:06:06 2014 +0000
Revision:
9:454e7da8ab65
Parent:
7:dd3cba61b34b
Child:
10:52b22bff409a
2014-10-20 Kalibratie van de arm werkt nog niet met de motor eraan. Wel alles in cases gezet zodat het overzichtelijk is en de abs in fabs veranderd zodat het een float blijft.

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 9:454e7da8ab65 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 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 9:454e7da8ab65 16 DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1
JKleinRot 9:454e7da8ab65 17 Encoder puls_motor_arm1(PTD2, PTD0); //Encoder pulsen tellen van motor arm 1
JKleinRot 9:454e7da8ab65 18 PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2
JKleinRot 9:454e7da8ab65 19 DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2
JKleinRot 9:454e7da8ab65 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 9:454e7da8ab65 27 //States definiëren
JKleinRot 9:454e7da8ab65 28 enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, KALIBRATIE_BICEPS, KALIBRATIE_TRICEPS}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
JKleinRot 9:454e7da8ab65 29 uint8_t state = RUST; //State is rust aan het begin
JKleinRot 9:454e7da8ab65 30
JKleinRot 6:3b6fad529520 31 //Gedefinieerde datatypen en naamgeving en beginwaarden
JKleinRot 6:3b6fad529520 32 char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm
JKleinRot 6:3b6fad529520 33 char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm
JKleinRot 0:859c89785d3f 34
JKleinRot 7:dd3cba61b34b 35 int puls_arm1_home = 363; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 1 op de thuispositie is
JKleinRot 7:dd3cba61b34b 36 int puls_arm2_home = 787; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 2 op de thuispositie is
JKleinRot 7:dd3cba61b34b 37 float pwm_to_motor_arm1; //PWM output naar motor arm 1
JKleinRot 7:dd3cba61b34b 38 float pwm_to_motor_arm2; //PWM output naar motor arm 2
JKleinRot 7:dd3cba61b34b 39 float xbk; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 7:dd3cba61b34b 40 int i; //
JKleinRot 7:dd3cba61b34b 41
JKleinRot 6:3b6fad529520 42 volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 6:3b6fad529520 43 void setregelaar_ticker_flag(){ //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
JKleinRot 6:3b6fad529520 44 regelaar_ticker_flag = true;
JKleinRot 1:e5e1eb9d0025 45 }
JKleinRot 1:e5e1eb9d0025 46
JKleinRot 6:3b6fad529520 47 volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 6:3b6fad529520 48 void setregelaar_EMG_flag(){ //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
JKleinRot 6:3b6fad529520 49 regelaar_EMG_flag = true;
JKleinRot 4:69540b9c0646 50 }
JKleinRot 4:69540b9c0646 51
JKleinRot 6:3b6fad529520 52 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 53 if (*in < min){ //Als de waarde kleiner is als het minimum wordt de waarde het minimum
JKleinRot 2:0cb899f2800a 54 *in = min;
JKleinRot 2:0cb899f2800a 55 }
JKleinRot 6:3b6fad529520 56 if (*in > max){ //Als de waarde groter is dan het maximum is de waarde het maximum
JKleinRot 2:0cb899f2800a 57 *in = max;
JKleinRot 2:0cb899f2800a 58 }
JKleinRot 6:3b6fad529520 59 else { //In alle andere gevallen is de waarde de waarde zelf
JKleinRot 2:0cb899f2800a 60 *in = *in;
JKleinRot 2:0cb899f2800a 61 }
JKleinRot 2:0cb899f2800a 62 }
JKleinRot 1:e5e1eb9d0025 63
JKleinRot 7:dd3cba61b34b 64 void arm1_naar_thuispositie(){
JKleinRot 7:dd3cba61b34b 65 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 66 keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 7:dd3cba61b34b 67
JKleinRot 7:dd3cba61b34b 68 if (pwm_to_motor_arm1 > 0){ //Als PWM is positief, dan richting 1
JKleinRot 7:dd3cba61b34b 69 dir_motor_arm1.write(1);
JKleinRot 7:dd3cba61b34b 70 }
JKleinRot 7:dd3cba61b34b 71 else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 72 dir_motor_arm1.write(0);
JKleinRot 7:dd3cba61b34b 73 }
JKleinRot 9:454e7da8ab65 74 pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 9:454e7da8ab65 75 pc.printf("pulsmotorgetposition %f\n\r", puls_motor_arm1.getPosition());
JKleinRot 9:454e7da8ab65 76 pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1);
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 9:454e7da8ab65 79 state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 9:454e7da8ab65 80 pc.printf("KALIBRATIE_ARM1 afgerond"); //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 9:454e7da8ab65 94 pwm_motor_arm2.write(fabs(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 9:454e7da8ab65 97 state = KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 9:454e7da8ab65 98 pc.printf("KALIBRATIE_ARM2 afgerond"); //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 9:454e7da8ab65 107
JKleinRot 9:454e7da8ab65 108 switch(state) //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case
JKleinRot 9:454e7da8ab65 109 {
JKleinRot 0:859c89785d3f 110
JKleinRot 9:454e7da8ab65 111 case RUST: //Aanzetten
JKleinRot 9:454e7da8ab65 112 {
JKleinRot 0:859c89785d3f 113 lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
JKleinRot 0:859c89785d3f 114 lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
JKleinRot 0:859c89785d3f 115 wait(2); //Twee seconden wachten
JKleinRot 9:454e7da8ab65 116 pc.printf("RUST afgerond"); //Tekst voor controle Aanzetten
JKleinRot 9:454e7da8ab65 117 state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
JKleinRot 9:454e7da8ab65 118 break; //Stopt acties in deze case
JKleinRot 0:859c89785d3f 119 }
JKleinRot 1:e5e1eb9d0025 120
JKleinRot 9:454e7da8ab65 121 case KALIBRATIE_ARM1: //Arm 1 naar thuispositie
JKleinRot 9:454e7da8ab65 122 {
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 9:454e7da8ab65 127 while(state == KALIBRATIE_ARM1){
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 9:454e7da8ab65 132 }
JKleinRot 9:454e7da8ab65 133 wait(1); //Een seconde wachten
JKleinRot 9:454e7da8ab65 134 break; //Stopt acties in deze case
JKleinRot 3:adc3052039e7 135 }
JKleinRot 3:adc3052039e7 136
JKleinRot 9:454e7da8ab65 137 case KALIBRATIE_ARM2: //Arm 2 naar thuispositie
JKleinRot 9:454e7da8ab65 138 {
JKleinRot 3:adc3052039e7 139 wait(1); //Een seconde wachten
JKleinRot 3:adc3052039e7 140
JKleinRot 3:adc3052039e7 141 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 3:adc3052039e7 142
JKleinRot 9:454e7da8ab65 143 while(state == KALIBRATIE_ARM2){
JKleinRot 6:3b6fad529520 144 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 145 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 3:adc3052039e7 146
JKleinRot 7:dd3cba61b34b 147 arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
JKleinRot 3:adc3052039e7 148 }
JKleinRot 6:3b6fad529520 149 wait(1); //Een seconde wachten
JKleinRot 9:454e7da8ab65 150 ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
JKleinRot 9:454e7da8ab65 151 break; //Stopt acties in deze case
JKleinRot 3:adc3052039e7 152 }
JKleinRot 4:69540b9c0646 153
JKleinRot 9:454e7da8ab65 154 case KALIBRATIE_BICEPS: //Kalibratie EMG signaal biceps
JKleinRot 9:454e7da8ab65 155 {
JKleinRot 9:454e7da8ab65 156 wait(1); //Een seconde wachten
JKleinRot 9:454e7da8ab65 157
JKleinRot 9:454e7da8ab65 158 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken
JKleinRot 4:69540b9c0646 159
JKleinRot 6:3b6fad529520 160 pc.printf("Ticker voor kalibratie compleet"); //Tekst voor controle Ticker voor kalibratie
JKleinRot 5:a1dcd605dd3d 161
JKleinRot 5:a1dcd605dd3d 162 //5 seconden EMG biceps meten
JKleinRot 5:a1dcd605dd3d 163
JKleinRot 6:3b6fad529520 164 wait(1); //Een seconde wachten
JKleinRot 6:3b6fad529520 165 lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm
JKleinRot 6:3b6fad529520 166 lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm
JKleinRot 5:a1dcd605dd3d 167
JKleinRot 7:dd3cba61b34b 168 for (i=0; i<1000; i++){
JKleinRot 7:dd3cba61b34b 169 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 170 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 5:a1dcd605dd3d 171
JKleinRot 6:3b6fad529520 172 xbk = EMG_bi.read(); //EMG signaal uitlezen
JKleinRot 5:a1dcd605dd3d 173 }
JKleinRot 5:a1dcd605dd3d 174
JKleinRot 4:69540b9c0646 175 }
JKleinRot 4:69540b9c0646 176
JKleinRot 4:69540b9c0646 177
JKleinRot 9:454e7da8ab65 178 }
JKleinRot 9:454e7da8ab65 179
JKleinRot 9:454e7da8ab65 180
JKleinRot 4:69540b9c0646 181
JKleinRot 4:69540b9c0646 182
JKleinRot 0:859c89785d3f 183 }