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

Committer:
JKleinRot
Date:
Tue Oct 21 14:51:23 2014 +0000
Revision:
11:e9b906b5f572
Parent:
10:52b22bff409a
Child:
12:996f9f8e3acc
Aantal pulsen naar home aangepast (delen door twee) en als #define gedaan

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 11:e9b906b5f572 7 #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
JKleinRot 11:e9b906b5f572 8 #define KP 0.001 //Factor proprotionele regelaar
JKleinRot 11:e9b906b5f572 9 #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
JKleinRot 11:e9b906b5f572 10 #define PULS_ARM1_HOME_KALIBRATIE 180 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
JKleinRot 11:e9b906b5f572 11 #define PULS_ARM2_HOME_KALIBRATIE 393 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
JKleinRot 0:859c89785d3f 12
JKleinRot 0:859c89785d3f 13 //Pinverdeling en naamgeving variabelen
JKleinRot 0:859c89785d3f 14 TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm
JKleinRot 0:859c89785d3f 15 MODSERIAL pc(USBTX, USBRX); //PC
JKleinRot 0:859c89785d3f 16
JKleinRot 1:e5e1eb9d0025 17 PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
JKleinRot 9:454e7da8ab65 18 DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1
JKleinRot 9:454e7da8ab65 19 Encoder puls_motor_arm1(PTD2, PTD0); //Encoder pulsen tellen van motor arm 1
JKleinRot 11:e9b906b5f572 20 PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2
JKleinRot 9:454e7da8ab65 21 DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2
JKleinRot 11:e9b906b5f572 22 Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2
JKleinRot 6:3b6fad529520 23 AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps
JKleinRot 1:e5e1eb9d0025 24
JKleinRot 6:3b6fad529520 25 Ticker ticker_regelaar; //Ticker voor regelaar motor
JKleinRot 6:3b6fad529520 26 Ticker ticker_EMG; //Ticker voor EMG meten
JKleinRot 1:e5e1eb9d0025 27
JKleinRot 9:454e7da8ab65 28 //States definiëren
JKleinRot 9:454e7da8ab65 29 enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, KALIBRATIE_BICEPS, KALIBRATIE_TRICEPS}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
JKleinRot 9:454e7da8ab65 30 uint8_t state = RUST; //State is rust aan het begin
JKleinRot 9:454e7da8ab65 31
JKleinRot 6:3b6fad529520 32 //Gedefinieerde datatypen en naamgeving en beginwaarden
JKleinRot 6:3b6fad529520 33 char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm
JKleinRot 6:3b6fad529520 34 char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm
JKleinRot 0:859c89785d3f 35
JKleinRot 11:e9b906b5f572 36 float pwm_to_motor_arm1; //PWM output naar motor arm 1
JKleinRot 11:e9b906b5f572 37 float pwm_to_motor_arm2; //PWM output naar motor arm 2
JKleinRot 11:e9b906b5f572 38 float xbk; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 11:e9b906b5f572 39 int i; //Voor for-loop
JKleinRot 7:dd3cba61b34b 40
JKleinRot 6:3b6fad529520 41 volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 42 void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
JKleinRot 10:52b22bff409a 43 {
JKleinRot 10:52b22bff409a 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 10:52b22bff409a 48 void setregelaar_EMG_flag() //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
JKleinRot 10:52b22bff409a 49 {
JKleinRot 10:52b22bff409a 50 regelaar_EMG_flag = true;
JKleinRot 4:69540b9c0646 51 }
JKleinRot 4:69540b9c0646 52
JKleinRot 10:52b22bff409a 53 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 54 {
JKleinRot 10:52b22bff409a 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 10:52b22bff409a 58 if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum
JKleinRot 2:0cb899f2800a 59 *in = max;
JKleinRot 10:52b22bff409a 60 } else { //In alle andere gevallen is de waarde de waarde zelf
JKleinRot 2:0cb899f2800a 61 *in = *in;
JKleinRot 2:0cb899f2800a 62 }
JKleinRot 2:0cb899f2800a 63 }
JKleinRot 1:e5e1eb9d0025 64
JKleinRot 10:52b22bff409a 65 void arm1_naar_thuispositie()
JKleinRot 10:52b22bff409a 66 {
JKleinRot 11:e9b906b5f572 67 pwm_to_motor_arm1 = (PULS_ARM1_HOME_KALIBRATIE - 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 10:52b22bff409a 69
JKleinRot 11:e9b906b5f572 70 if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 10:52b22bff409a 71 dir_motor_arm1.write(1);
JKleinRot 11:e9b906b5f572 72 } else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 73 dir_motor_arm1.write(0);
JKleinRot 7:dd3cba61b34b 74 }
JKleinRot 11:e9b906b5f572 75 pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 10:52b22bff409a 76 pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition());
JKleinRot 9:454e7da8ab65 77 pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1);
JKleinRot 10:52b22bff409a 78
JKleinRot 11:e9b906b5f572 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 11:e9b906b5f572 80 state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 11:e9b906b5f572 81 pc.printf("KALIBRATIE_ARM1 afgerond"); //Tekst voor controle Arm 1 naar thuispositie
JKleinRot 10:52b22bff409a 82 }
JKleinRot 10:52b22bff409a 83 }
JKleinRot 7:dd3cba61b34b 84
JKleinRot 10:52b22bff409a 85 void arm2_naar_thuispositie()
JKleinRot 10:52b22bff409a 86 {
JKleinRot 11:e9b906b5f572 87 pwm_to_motor_arm2 = (PULS_ARM2_HOME_KALIBRATIE - 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 88 keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 10:52b22bff409a 89
JKleinRot 11:e9b906b5f572 90 if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 7:dd3cba61b34b 91 dir_motor_arm2.write(1);
JKleinRot 11:e9b906b5f572 92 } else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 93 dir_motor_arm2.write(0);
JKleinRot 7:dd3cba61b34b 94 }
JKleinRot 11:e9b906b5f572 95 pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 10:52b22bff409a 96
JKleinRot 11:e9b906b5f572 97 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 11:e9b906b5f572 98 state = KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 11:e9b906b5f572 99 pc.printf("KALIBRATIE_ARM2 afgerond"); //Tekst voor controle Arm 2 naar thuispositie
JKleinRot 7:dd3cba61b34b 100 }
JKleinRot 7:dd3cba61b34b 101 }
JKleinRot 0:859c89785d3f 102
JKleinRot 0:859c89785d3f 103
JKleinRot 10:52b22bff409a 104 int main()
JKleinRot 10:52b22bff409a 105 {
JKleinRot 11:e9b906b5f572 106 while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
JKleinRot 10:52b22bff409a 107 //PC baud rate instellen
JKleinRot 10:52b22bff409a 108 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 10:52b22bff409a 109
JKleinRot 10:52b22bff409a 110 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 111
JKleinRot 11:e9b906b5f572 112 case RUST: { //Aanzetten
JKleinRot 10:52b22bff409a 113 lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
JKleinRot 10:52b22bff409a 114 lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
JKleinRot 10:52b22bff409a 115 wait(2); //Twee seconden wachten
JKleinRot 10:52b22bff409a 116 pc.printf("RUST afgerond"); //Tekst voor controle Aanzetten
JKleinRot 10:52b22bff409a 117 state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
JKleinRot 10:52b22bff409a 118 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 119 }
JKleinRot 10:52b22bff409a 120
JKleinRot 10:52b22bff409a 121 case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie
JKleinRot 10:52b22bff409a 122 pc.printf("kalibratie-arm1");
JKleinRot 10:52b22bff409a 123 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 124 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 125
JKleinRot 10:52b22bff409a 126 while(state == KALIBRATIE_ARM1) {
JKleinRot 10:52b22bff409a 127 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 128 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 129
JKleinRot 10:52b22bff409a 130 arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 131 }
JKleinRot 10:52b22bff409a 132 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 133 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 134 }
JKleinRot 10:52b22bff409a 135
JKleinRot 11:e9b906b5f572 136 case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
JKleinRot 10:52b22bff409a 137 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 138
JKleinRot 10:52b22bff409a 139 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 140
JKleinRot 10:52b22bff409a 141 while(state == KALIBRATIE_ARM2) {
JKleinRot 10:52b22bff409a 142 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 143 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 144
JKleinRot 10:52b22bff409a 145 arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 146 }
JKleinRot 11:e9b906b5f572 147 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 148 ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
JKleinRot 10:52b22bff409a 149 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 150 }
JKleinRot 10:52b22bff409a 151
JKleinRot 10:52b22bff409a 152 case KALIBRATIE_BICEPS: { //Kalibratie EMG signaal biceps
JKleinRot 10:52b22bff409a 153 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 154
JKleinRot 10:52b22bff409a 155 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken
JKleinRot 10:52b22bff409a 156
JKleinRot 10:52b22bff409a 157 pc.printf("Ticker voor kalibratie compleet"); //Tekst voor controle Ticker voor kalibratie
JKleinRot 10:52b22bff409a 158
JKleinRot 10:52b22bff409a 159 //5 seconden EMG biceps meten
JKleinRot 10:52b22bff409a 160
JKleinRot 10:52b22bff409a 161 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 162 lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm
JKleinRot 10:52b22bff409a 163 lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm
JKleinRot 10:52b22bff409a 164
JKleinRot 10:52b22bff409a 165 for (i=0; i<1000; i++) {
JKleinRot 10:52b22bff409a 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 10:52b22bff409a 167 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 168
JKleinRot 10:52b22bff409a 169 xbk = EMG_bi.read(); //EMG signaal uitlezen
JKleinRot 10:52b22bff409a 170 }
JKleinRot 10:52b22bff409a 171 break;
JKleinRot 10:52b22bff409a 172
JKleinRot 10:52b22bff409a 173 }
JKleinRot 11:e9b906b5f572 174 default: { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
JKleinRot 11:e9b906b5f572 175 state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
JKleinRot 10:52b22bff409a 176 }
JKleinRot 9:454e7da8ab65 177 }
JKleinRot 10:52b22bff409a 178 pc.printf("state: %u\n",state);
JKleinRot 3:adc3052039e7 179 }
JKleinRot 0:859c89785d3f 180 }