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

Committer:
JKleinRot
Date:
Thu Oct 16 12:19:15 2014 +0000
Revision:
3:adc3052039e7
Parent:
2:0cb899f2800a
Child:
4:69540b9c0646
2014-10-16 Arm 2 naar thuispositie, zelfde script als Arm 1, aangepast en nieuwe dingen gedefinieerd.

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 0:859c89785d3f 9
JKleinRot 0:859c89785d3f 10 //Pinverdeling en naamgeving variabelen
JKleinRot 0:859c89785d3f 11 TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm
JKleinRot 0:859c89785d3f 12 MODSERIAL pc(USBTX, USBRX); //PC
JKleinRot 0:859c89785d3f 13
JKleinRot 1:e5e1eb9d0025 14 PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
JKleinRot 1:e5e1eb9d0025 15 DigitalOut dir_motor_arm1(PTD1); //Richting van motor arm 1
JKleinRot 1:e5e1eb9d0025 16 Encoder puls_motor_arm1(PTD0, PTC9); //Encoder pulsen tellen van motor arm 1
JKleinRot 3:adc3052039e7 17 PwmOut pwm_motor_arm2(PTA12);
JKleinRot 3:adc3052039e7 18 DigitalOut dir_motor_arm2(PTD3);
JKleinRot 3:adc3052039e7 19 Encoder puls_motor_arm2(PTD4, PTC8);
JKleinRot 1:e5e1eb9d0025 20
JKleinRot 1:e5e1eb9d0025 21 Ticker ticker_regelaar;
JKleinRot 1:e5e1eb9d0025 22
JKleinRot 0:859c89785d3f 23 //Gedefinieerde datatypen en naamgeving
JKleinRot 0:859c89785d3f 24 bool rust = false; //Bool voor controleren volgorde in programma
JKleinRot 3:adc3052039e7 25 bool kalibratie_positie_arm1 = false; //Bool voor controleren volgorde in programma
JKleinRot 3:adc3052039e7 26 bool kalibratie_positie_arm2 = false;
JKleinRot 0:859c89785d3f 27 bool kalibratie_EMG = false; //Bool voor controleren volgorde in programma
JKleinRot 0:859c89785d3f 28
JKleinRot 0:859c89785d3f 29 char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm
JKleinRot 0:859c89785d3f 30 char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm
JKleinRot 0:859c89785d3f 31
JKleinRot 1:e5e1eb9d0025 32 volatile bool regelaar_ticker_flag;
JKleinRot 1:e5e1eb9d0025 33 void setregelaar_ticker_flag(){
JKleinRot 1:e5e1eb9d0025 34 regelaar_ticker_flag = true;
JKleinRot 1:e5e1eb9d0025 35 }
JKleinRot 1:e5e1eb9d0025 36
JKleinRot 2:0cb899f2800a 37 void keep_in_range(float * in, float min, float max){
JKleinRot 2:0cb899f2800a 38 if (*in < min){
JKleinRot 2:0cb899f2800a 39 *in = min;
JKleinRot 2:0cb899f2800a 40 }
JKleinRot 2:0cb899f2800a 41 if (*in > max){
JKleinRot 2:0cb899f2800a 42 *in = max;
JKleinRot 2:0cb899f2800a 43 }
JKleinRot 2:0cb899f2800a 44 else {
JKleinRot 2:0cb899f2800a 45 *in = *in;
JKleinRot 2:0cb899f2800a 46 }
JKleinRot 2:0cb899f2800a 47 }
JKleinRot 2:0cb899f2800a 48
JKleinRot 1:e5e1eb9d0025 49
JKleinRot 1:e5e1eb9d0025 50 int puls_arm1_home = 363;
JKleinRot 3:adc3052039e7 51 int puls_arm2_home = 787;
JKleinRot 1:e5e1eb9d0025 52 float pwm_to_motor_arm1;
JKleinRot 3:adc3052039e7 53 float pwm_to_motor_arm2;
JKleinRot 1:e5e1eb9d0025 54
JKleinRot 0:859c89785d3f 55 //Beginwaarden voor variabelen
JKleinRot 0:859c89785d3f 56
JKleinRot 0:859c89785d3f 57
JKleinRot 0:859c89785d3f 58 int main() {
JKleinRot 0:859c89785d3f 59
JKleinRot 0:859c89785d3f 60 //PC baud rate instellen
JKleinRot 0:859c89785d3f 61 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 0:859c89785d3f 62
JKleinRot 0:859c89785d3f 63 //Aanzetten
JKleinRot 3:adc3052039e7 64 if (rust == false && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG == false){
JKleinRot 0:859c89785d3f 65 lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
JKleinRot 0:859c89785d3f 66 lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
JKleinRot 0:859c89785d3f 67 wait(2); //Twee seconden wachten
JKleinRot 0:859c89785d3f 68 pc.printf("Aanzetten compleet"); //Tekst voor controle Aanzetten
JKleinRot 0:859c89785d3f 69 rust = true; //Rust wordt waar zodat door kan worden gegaan naar het volgende deel
JKleinRot 0:859c89785d3f 70 }
JKleinRot 1:e5e1eb9d0025 71
JKleinRot 1:e5e1eb9d0025 72
JKleinRot 1:e5e1eb9d0025 73
JKleinRot 1:e5e1eb9d0025 74 //Arm 1 naar thuispositie
JKleinRot 3:adc3052039e7 75 if (rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG == false){
JKleinRot 1:e5e1eb9d0025 76 wait(1); //Een seconde wachten
JKleinRot 1:e5e1eb9d0025 77
JKleinRot 1:e5e1eb9d0025 78 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 1:e5e1eb9d0025 79
JKleinRot 3:adc3052039e7 80 while(rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG == false) {
JKleinRot 1:e5e1eb9d0025 81 while(regelaar_ticker_flag != true) ;
JKleinRot 1:e5e1eb9d0025 82 regelaar_ticker_flag = false;
JKleinRot 1:e5e1eb9d0025 83
JKleinRot 1:e5e1eb9d0025 84 pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP;
JKleinRot 1:e5e1eb9d0025 85 keep_in_range(&pwm_to_motor_arm1, -1, 1);
JKleinRot 1:e5e1eb9d0025 86
JKleinRot 1:e5e1eb9d0025 87 if (pwm_to_motor_arm1 > 0){
JKleinRot 1:e5e1eb9d0025 88 dir_motor_arm1.write(1);
JKleinRot 1:e5e1eb9d0025 89 }
JKleinRot 1:e5e1eb9d0025 90 else {
JKleinRot 1:e5e1eb9d0025 91 dir_motor_arm1.write(0);
JKleinRot 1:e5e1eb9d0025 92 }
JKleinRot 1:e5e1eb9d0025 93 pwm_motor_arm1.write(abs(pwm_to_motor_arm1));
JKleinRot 2:0cb899f2800a 94
JKleinRot 2:0cb899f2800a 95 if (pwm_to_motor_arm1 == 0) {
JKleinRot 3:adc3052039e7 96 kalibratie_positie_arm1 = true;
JKleinRot 2:0cb899f2800a 97 pc.printf("Arm 1 naar thuispositie compleet");
JKleinRot 2:0cb899f2800a 98 }
JKleinRot 1:e5e1eb9d0025 99 wait(1);
JKleinRot 2:0cb899f2800a 100
JKleinRot 1:e5e1eb9d0025 101 }
JKleinRot 3:adc3052039e7 102 }
JKleinRot 3:adc3052039e7 103
JKleinRot 3:adc3052039e7 104 //Arm 2 naar thuispositie
JKleinRot 3:adc3052039e7 105 if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG == false){
JKleinRot 3:adc3052039e7 106 wait(1); //Een seconde wachten
JKleinRot 3:adc3052039e7 107
JKleinRot 3:adc3052039e7 108 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 3:adc3052039e7 109
JKleinRot 3:adc3052039e7 110 while(rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG == false) {
JKleinRot 3:adc3052039e7 111 while(regelaar_ticker_flag != true) ;
JKleinRot 3:adc3052039e7 112 regelaar_ticker_flag = false;
JKleinRot 3:adc3052039e7 113
JKleinRot 3:adc3052039e7 114 pwm_to_motor_arm2 = (puls_arm2_home - puls_motor_arm2.getPosition())*KP;
JKleinRot 3:adc3052039e7 115 keep_in_range(&pwm_to_motor_arm2, -1, 1);
JKleinRot 3:adc3052039e7 116
JKleinRot 3:adc3052039e7 117 if (pwm_to_motor_arm2 > 0){
JKleinRot 3:adc3052039e7 118 dir_motor_arm2.write(1);
JKleinRot 3:adc3052039e7 119 }
JKleinRot 3:adc3052039e7 120 else {
JKleinRot 3:adc3052039e7 121 dir_motor_arm2.write(0);
JKleinRot 3:adc3052039e7 122 }
JKleinRot 3:adc3052039e7 123 pwm_motor_arm2.write(abs(pwm_to_motor_arm2));
JKleinRot 3:adc3052039e7 124
JKleinRot 3:adc3052039e7 125 if (pwm_to_motor_arm2 == 0) {
JKleinRot 3:adc3052039e7 126 kalibratie_positie_arm2 = true;
JKleinRot 3:adc3052039e7 127 pc.printf("Arm 2 naar thuispositie compleet");
JKleinRot 3:adc3052039e7 128 }
JKleinRot 3:adc3052039e7 129 wait(1);
JKleinRot 3:adc3052039e7 130 }
JKleinRot 3:adc3052039e7 131 }
JKleinRot 0:859c89785d3f 132 }