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@5:a1dcd605dd3d, 2014-10-16 (annotated)

Committer:
JKleinRot
Date:
Thu Oct 16 13:32:57 2014 +0000
Revision:
5:a1dcd605dd3d
Parent:
4:69540b9c0646
Child:
6:3b6fad529520
2014-10-16. EMG meten voor kalibratie, eerste poging. For loop voor 5 seconden meten lukt niet

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 1:e5e1eb9d0025 7 #define SAMPLETIME_REGELAAR 0.005
JKleinRot 1:e5e1eb9d0025 8 #define KP 1
JKleinRot 4:69540b9c0646 9 #define SAMPLETIME_EMG 0.005
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 3:adc3052039e7 18 PwmOut pwm_motor_arm2(PTA12);
JKleinRot 3:adc3052039e7 19 DigitalOut dir_motor_arm2(PTD3);
JKleinRot 3:adc3052039e7 20 Encoder puls_motor_arm2(PTD4, PTC8);
JKleinRot 4:69540b9c0646 21 AnalogIn EMG_bi(PTB1);
JKleinRot 1:e5e1eb9d0025 22
JKleinRot 4:69540b9c0646 23
JKleinRot 4:69540b9c0646 24 Ticker ticker_regelaar;
JKleinRot 4:69540b9c0646 25 Ticker ticker_EMG;
JKleinRot 1:e5e1eb9d0025 26
JKleinRot 0:859c89785d3f 27 //Gedefinieerde datatypen en naamgeving
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 3:adc3052039e7 30 bool kalibratie_positie_arm2 = false;
JKleinRot 4:69540b9c0646 31 bool kalibratie_EMG_bi = false; //Bool voor controleren volgorde in programma
JKleinRot 4:69540b9c0646 32 bool kalibratie_EMG_tri = false;
JKleinRot 0:859c89785d3f 33
JKleinRot 0:859c89785d3f 34 char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm
JKleinRot 0:859c89785d3f 35 char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm
JKleinRot 0:859c89785d3f 36
JKleinRot 1:e5e1eb9d0025 37 volatile bool regelaar_ticker_flag;
JKleinRot 1:e5e1eb9d0025 38 void setregelaar_ticker_flag(){
JKleinRot 1:e5e1eb9d0025 39 regelaar_ticker_flag = true;
JKleinRot 1:e5e1eb9d0025 40 }
JKleinRot 1:e5e1eb9d0025 41
JKleinRot 4:69540b9c0646 42 volatile bool regelaar_EMG_flag;
JKleinRot 4:69540b9c0646 43 void setregelaar_EMG_flag(){
JKleinRot 4:69540b9c0646 44 regelaar_EMG_flag = true;
JKleinRot 4:69540b9c0646 45 }
JKleinRot 4:69540b9c0646 46
JKleinRot 2:0cb899f2800a 47 void keep_in_range(float * in, float min, float max){
JKleinRot 2:0cb899f2800a 48 if (*in < min){
JKleinRot 2:0cb899f2800a 49 *in = min;
JKleinRot 2:0cb899f2800a 50 }
JKleinRot 2:0cb899f2800a 51 if (*in > max){
JKleinRot 2:0cb899f2800a 52 *in = max;
JKleinRot 2:0cb899f2800a 53 }
JKleinRot 2:0cb899f2800a 54 else {
JKleinRot 2:0cb899f2800a 55 *in = *in;
JKleinRot 2:0cb899f2800a 56 }
JKleinRot 2:0cb899f2800a 57 }
JKleinRot 2:0cb899f2800a 58
JKleinRot 1:e5e1eb9d0025 59
JKleinRot 1:e5e1eb9d0025 60 int puls_arm1_home = 363;
JKleinRot 3:adc3052039e7 61 int puls_arm2_home = 787;
JKleinRot 1:e5e1eb9d0025 62 float pwm_to_motor_arm1;
JKleinRot 3:adc3052039e7 63 float pwm_to_motor_arm2;
JKleinRot 5:a1dcd605dd3d 64 float xbk;
JKleinRot 5:a1dcd605dd3d 65 int i = 0;
JKleinRot 1:e5e1eb9d0025 66
JKleinRot 0:859c89785d3f 67 //Beginwaarden voor variabelen
JKleinRot 0:859c89785d3f 68
JKleinRot 0:859c89785d3f 69
JKleinRot 0:859c89785d3f 70 int main() {
JKleinRot 0:859c89785d3f 71
JKleinRot 0:859c89785d3f 72 //PC baud rate instellen
JKleinRot 0:859c89785d3f 73 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 0:859c89785d3f 74
JKleinRot 0:859c89785d3f 75 //Aanzetten
JKleinRot 4:69540b9c0646 76 if (rust == false && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 0:859c89785d3f 77 lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
JKleinRot 0:859c89785d3f 78 lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
JKleinRot 0:859c89785d3f 79 wait(2); //Twee seconden wachten
JKleinRot 0:859c89785d3f 80 pc.printf("Aanzetten compleet"); //Tekst voor controle Aanzetten
JKleinRot 0:859c89785d3f 81 rust = true; //Rust wordt waar zodat door kan worden gegaan naar het volgende deel
JKleinRot 0:859c89785d3f 82 }
JKleinRot 1:e5e1eb9d0025 83
JKleinRot 1:e5e1eb9d0025 84
JKleinRot 1:e5e1eb9d0025 85
JKleinRot 1:e5e1eb9d0025 86 //Arm 1 naar thuispositie
JKleinRot 4:69540b9c0646 87 if (rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 1:e5e1eb9d0025 88 wait(1); //Een seconde wachten
JKleinRot 1:e5e1eb9d0025 89
JKleinRot 1:e5e1eb9d0025 90 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 1:e5e1eb9d0025 91
JKleinRot 4:69540b9c0646 92 while(rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) {
JKleinRot 1:e5e1eb9d0025 93 while(regelaar_ticker_flag != true) ;
JKleinRot 1:e5e1eb9d0025 94 regelaar_ticker_flag = false;
JKleinRot 1:e5e1eb9d0025 95
JKleinRot 1:e5e1eb9d0025 96 pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP;
JKleinRot 1:e5e1eb9d0025 97 keep_in_range(&pwm_to_motor_arm1, -1, 1);
JKleinRot 1:e5e1eb9d0025 98
JKleinRot 1:e5e1eb9d0025 99 if (pwm_to_motor_arm1 > 0){
JKleinRot 1:e5e1eb9d0025 100 dir_motor_arm1.write(1);
JKleinRot 1:e5e1eb9d0025 101 }
JKleinRot 1:e5e1eb9d0025 102 else {
JKleinRot 1:e5e1eb9d0025 103 dir_motor_arm1.write(0);
JKleinRot 1:e5e1eb9d0025 104 }
JKleinRot 1:e5e1eb9d0025 105 pwm_motor_arm1.write(abs(pwm_to_motor_arm1));
JKleinRot 2:0cb899f2800a 106
JKleinRot 2:0cb899f2800a 107 if (pwm_to_motor_arm1 == 0) {
JKleinRot 3:adc3052039e7 108 kalibratie_positie_arm1 = true;
JKleinRot 2:0cb899f2800a 109 pc.printf("Arm 1 naar thuispositie compleet");
JKleinRot 2:0cb899f2800a 110 }
JKleinRot 1:e5e1eb9d0025 111 wait(1);
JKleinRot 2:0cb899f2800a 112
JKleinRot 1:e5e1eb9d0025 113 }
JKleinRot 3:adc3052039e7 114 }
JKleinRot 3:adc3052039e7 115
JKleinRot 3:adc3052039e7 116 //Arm 2 naar thuispositie
JKleinRot 4:69540b9c0646 117 if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 3:adc3052039e7 118 wait(1); //Een seconde wachten
JKleinRot 3:adc3052039e7 119
JKleinRot 3:adc3052039e7 120 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 3:adc3052039e7 121
JKleinRot 4:69540b9c0646 122 while(rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) {
JKleinRot 3:adc3052039e7 123 while(regelaar_ticker_flag != true) ;
JKleinRot 3:adc3052039e7 124 regelaar_ticker_flag = false;
JKleinRot 3:adc3052039e7 125
JKleinRot 3:adc3052039e7 126 pwm_to_motor_arm2 = (puls_arm2_home - puls_motor_arm2.getPosition())*KP;
JKleinRot 3:adc3052039e7 127 keep_in_range(&pwm_to_motor_arm2, -1, 1);
JKleinRot 3:adc3052039e7 128
JKleinRot 3:adc3052039e7 129 if (pwm_to_motor_arm2 > 0){
JKleinRot 3:adc3052039e7 130 dir_motor_arm2.write(1);
JKleinRot 3:adc3052039e7 131 }
JKleinRot 3:adc3052039e7 132 else {
JKleinRot 3:adc3052039e7 133 dir_motor_arm2.write(0);
JKleinRot 3:adc3052039e7 134 }
JKleinRot 3:adc3052039e7 135 pwm_motor_arm2.write(abs(pwm_to_motor_arm2));
JKleinRot 3:adc3052039e7 136
JKleinRot 3:adc3052039e7 137 if (pwm_to_motor_arm2 == 0) {
JKleinRot 3:adc3052039e7 138 kalibratie_positie_arm2 = true;
JKleinRot 3:adc3052039e7 139 pc.printf("Arm 2 naar thuispositie compleet");
JKleinRot 3:adc3052039e7 140 }
JKleinRot 3:adc3052039e7 141 }
JKleinRot 5:a1dcd605dd3d 142 wait(1);
JKleinRot 5:a1dcd605dd3d 143 ticker_regelaar.detach();
JKleinRot 3:adc3052039e7 144 }
JKleinRot 4:69540b9c0646 145
JKleinRot 4:69540b9c0646 146 //Ticker voor kalibratie
JKleinRot 4:69540b9c0646 147 if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == true && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
JKleinRot 4:69540b9c0646 148
JKleinRot 4:69540b9c0646 149 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);
JKleinRot 4:69540b9c0646 150 pc.printf("Ticker voor kalibratie compleet");
JKleinRot 5:a1dcd605dd3d 151
JKleinRot 5:a1dcd605dd3d 152 //5 seconden EMG biceps meten
JKleinRot 5:a1dcd605dd3d 153
JKleinRot 5:a1dcd605dd3d 154 wait(1);
JKleinRot 5:a1dcd605dd3d 155 lcd_r1 = " EMG kalibratie ";
JKleinRot 5:a1dcd605dd3d 156 lcd_r2 = " Span biceps aan";
JKleinRot 5:a1dcd605dd3d 157
JKleinRot 5:a1dcd605dd3d 158 for (i=0 ,i<5000, i++){
JKleinRot 5:a1dcd605dd3d 159 while(setregelaar_EMG_flag != true) ;
JKleinRot 5:a1dcd605dd3d 160 regelaar_EMG_flag = false;
JKleinRot 5:a1dcd605dd3d 161
JKleinRot 5:a1dcd605dd3d 162 xbk = EMG_bi.read();
JKleinRot 5:a1dcd605dd3d 163 }
JKleinRot 5:a1dcd605dd3d 164
JKleinRot 4:69540b9c0646 165 }
JKleinRot 4:69540b9c0646 166
JKleinRot 4:69540b9c0646 167
JKleinRot 4:69540b9c0646 168
JKleinRot 4:69540b9c0646 169
JKleinRot 0:859c89785d3f 170 }