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@10:52b22bff409a, 2014-10-21 (annotated)

Committer:
JKleinRot
Date:
Tue Oct 21 11:57:27 2014 +0000
Revision:
10:52b22bff409a
Parent:
9:454e7da8ab65
Child:
11:e9b906b5f572
State machine now working

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 10:52b22bff409a 43 void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
JKleinRot 10:52b22bff409a 44 {
JKleinRot 10:52b22bff409a 45 regelaar_ticker_flag = true;
JKleinRot 1:e5e1eb9d0025 46 }
JKleinRot 1:e5e1eb9d0025 47
JKleinRot 6:3b6fad529520 48 volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 49 void setregelaar_EMG_flag() //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
JKleinRot 10:52b22bff409a 50 {
JKleinRot 10:52b22bff409a 51 regelaar_EMG_flag = true;
JKleinRot 4:69540b9c0646 52 }
JKleinRot 4:69540b9c0646 53
JKleinRot 10:52b22bff409a 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 10:52b22bff409a 55 {
JKleinRot 10:52b22bff409a 56 if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum
JKleinRot 2:0cb899f2800a 57 *in = min;
JKleinRot 2:0cb899f2800a 58 }
JKleinRot 10:52b22bff409a 59 if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum
JKleinRot 2:0cb899f2800a 60 *in = max;
JKleinRot 10:52b22bff409a 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 10:52b22bff409a 66 void arm1_naar_thuispositie()
JKleinRot 10:52b22bff409a 67 {
JKleinRot 7:dd3cba61b34b 68 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 69 keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 10:52b22bff409a 70
JKleinRot 10:52b22bff409a 71 if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 10:52b22bff409a 72 dir_motor_arm1.write(1);
JKleinRot 10:52b22bff409a 73 } else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 74 dir_motor_arm1.write(0);
JKleinRot 7:dd3cba61b34b 75 }
JKleinRot 9:454e7da8ab65 76 pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 10:52b22bff409a 77 pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition());
JKleinRot 9:454e7da8ab65 78 pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1);
JKleinRot 10:52b22bff409a 79
JKleinRot 7:dd3cba61b34b 80 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 81 state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 9:454e7da8ab65 82 pc.printf("KALIBRATIE_ARM1 afgerond"); //Tekst voor controle Arm 1 naar thuispositie
JKleinRot 10:52b22bff409a 83 }
JKleinRot 10:52b22bff409a 84 }
JKleinRot 7:dd3cba61b34b 85
JKleinRot 10:52b22bff409a 86 void arm2_naar_thuispositie()
JKleinRot 10:52b22bff409a 87 {
JKleinRot 7:dd3cba61b34b 88 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 89 keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 10:52b22bff409a 90
JKleinRot 10:52b22bff409a 91 if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 7:dd3cba61b34b 92 dir_motor_arm2.write(1);
JKleinRot 10:52b22bff409a 93 } else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 94 dir_motor_arm2.write(0);
JKleinRot 7:dd3cba61b34b 95 }
JKleinRot 9:454e7da8ab65 96 pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 10:52b22bff409a 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 9:454e7da8ab65 99 state = KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 9:454e7da8ab65 100 pc.printf("KALIBRATIE_ARM2 afgerond"); //Tekst voor controle Arm 2 naar thuispositie
JKleinRot 7:dd3cba61b34b 101 }
JKleinRot 7:dd3cba61b34b 102 }
JKleinRot 0:859c89785d3f 103
JKleinRot 0:859c89785d3f 104
JKleinRot 10:52b22bff409a 105 int main()
JKleinRot 10:52b22bff409a 106 {
JKleinRot 10:52b22bff409a 107 while(1) {
JKleinRot 10:52b22bff409a 108 //PC baud rate instellen
JKleinRot 10:52b22bff409a 109 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 10:52b22bff409a 110
JKleinRot 10:52b22bff409a 111 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 10:52b22bff409a 112
JKleinRot 10:52b22bff409a 113 case RUST: { //Aanzetten
JKleinRot 10:52b22bff409a 114 lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
JKleinRot 10:52b22bff409a 115 lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
JKleinRot 10:52b22bff409a 116 wait(2); //Twee seconden wachten
JKleinRot 10:52b22bff409a 117 pc.printf("RUST afgerond"); //Tekst voor controle Aanzetten
JKleinRot 10:52b22bff409a 118 state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
JKleinRot 10:52b22bff409a 119 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 120 }
JKleinRot 10:52b22bff409a 121
JKleinRot 10:52b22bff409a 122 case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie
JKleinRot 10:52b22bff409a 123 pc.printf("kalibratie-arm1");
JKleinRot 10:52b22bff409a 124 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 125 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 126
JKleinRot 10:52b22bff409a 127 while(state == KALIBRATIE_ARM1) {
JKleinRot 10:52b22bff409a 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 10:52b22bff409a 129 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 130
JKleinRot 10:52b22bff409a 131 arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 132 }
JKleinRot 10:52b22bff409a 133 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 134 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 135 }
JKleinRot 10:52b22bff409a 136
JKleinRot 10:52b22bff409a 137 case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
JKleinRot 10:52b22bff409a 138 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 139
JKleinRot 10:52b22bff409a 140 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 141
JKleinRot 10:52b22bff409a 142 while(state == KALIBRATIE_ARM2) {
JKleinRot 10:52b22bff409a 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 10:52b22bff409a 144 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 145
JKleinRot 10:52b22bff409a 146 arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 147 }
JKleinRot 10:52b22bff409a 148 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 149 ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
JKleinRot 10:52b22bff409a 150 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 151 }
JKleinRot 10:52b22bff409a 152
JKleinRot 10:52b22bff409a 153 case KALIBRATIE_BICEPS: { //Kalibratie EMG signaal biceps
JKleinRot 10:52b22bff409a 154 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 155
JKleinRot 10:52b22bff409a 156 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken
JKleinRot 10:52b22bff409a 157
JKleinRot 10:52b22bff409a 158 pc.printf("Ticker voor kalibratie compleet"); //Tekst voor controle Ticker voor kalibratie
JKleinRot 10:52b22bff409a 159
JKleinRot 10:52b22bff409a 160 //5 seconden EMG biceps meten
JKleinRot 10:52b22bff409a 161
JKleinRot 10:52b22bff409a 162 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 163 lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm
JKleinRot 10:52b22bff409a 164 lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm
JKleinRot 10:52b22bff409a 165
JKleinRot 10:52b22bff409a 166 for (i=0; i<1000; i++) {
JKleinRot 10:52b22bff409a 167 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 10:52b22bff409a 168 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 169
JKleinRot 10:52b22bff409a 170 xbk = EMG_bi.read(); //EMG signaal uitlezen
JKleinRot 10:52b22bff409a 171 }
JKleinRot 10:52b22bff409a 172 break;
JKleinRot 10:52b22bff409a 173
JKleinRot 10:52b22bff409a 174 }
JKleinRot 10:52b22bff409a 175 default: {
JKleinRot 10:52b22bff409a 176 state = RUST;
JKleinRot 10:52b22bff409a 177 }
JKleinRot 9:454e7da8ab65 178 }
JKleinRot 10:52b22bff409a 179 pc.printf("state: %u\n",state);
JKleinRot 3:adc3052039e7 180 }
JKleinRot 0:859c89785d3f 181 }