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@2:0cb899f2800a, 2014-10-16 (annotated)

Committer:
JKleinRot
Date:
Thu Oct 16 11:53:26 2014 +0000
Revision:
2:0cb899f2800a
Parent:
1:e5e1eb9d0025
Child:
3:adc3052039e7
2014-10-16 tweede poging arm 1 naar thuispositie. Nu werkende functie, niet zeker of het echt werkt.

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 1:e5e1eb9d0025 17
JKleinRot 1:e5e1eb9d0025 18 Ticker ticker_regelaar;
JKleinRot 1:e5e1eb9d0025 19
JKleinRot 0:859c89785d3f 20 //Gedefinieerde datatypen en naamgeving
JKleinRot 0:859c89785d3f 21 bool rust = false; //Bool voor controleren volgorde in programma
JKleinRot 0:859c89785d3f 22 bool kalibratie_positie = false; //Bool voor controleren volgorde in programma
JKleinRot 0:859c89785d3f 23 bool kalibratie_EMG = false; //Bool voor controleren volgorde in programma
JKleinRot 0:859c89785d3f 24
JKleinRot 0:859c89785d3f 25 char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm
JKleinRot 0:859c89785d3f 26 char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm
JKleinRot 0:859c89785d3f 27
JKleinRot 1:e5e1eb9d0025 28 volatile bool regelaar_ticker_flag;
JKleinRot 1:e5e1eb9d0025 29 void setregelaar_ticker_flag(){
JKleinRot 1:e5e1eb9d0025 30 regelaar_ticker_flag = true;
JKleinRot 1:e5e1eb9d0025 31 }
JKleinRot 1:e5e1eb9d0025 32
JKleinRot 2:0cb899f2800a 33 void keep_in_range(float * in, float min, float max){
JKleinRot 2:0cb899f2800a 34 if (*in < min){
JKleinRot 2:0cb899f2800a 35 *in = min;
JKleinRot 2:0cb899f2800a 36 }
JKleinRot 2:0cb899f2800a 37 if (*in > max){
JKleinRot 2:0cb899f2800a 38 *in = max;
JKleinRot 2:0cb899f2800a 39 }
JKleinRot 2:0cb899f2800a 40 else {
JKleinRot 2:0cb899f2800a 41 *in = *in;
JKleinRot 2:0cb899f2800a 42 }
JKleinRot 2:0cb899f2800a 43 }
JKleinRot 2:0cb899f2800a 44
JKleinRot 1:e5e1eb9d0025 45
JKleinRot 1:e5e1eb9d0025 46 int puls_arm1_home = 363;
JKleinRot 1:e5e1eb9d0025 47 float pwm_to_motor_arm1;
JKleinRot 1:e5e1eb9d0025 48
JKleinRot 0:859c89785d3f 49 //Beginwaarden voor variabelen
JKleinRot 0:859c89785d3f 50
JKleinRot 0:859c89785d3f 51
JKleinRot 0:859c89785d3f 52 int main() {
JKleinRot 0:859c89785d3f 53
JKleinRot 0:859c89785d3f 54 //PC baud rate instellen
JKleinRot 0:859c89785d3f 55 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 0:859c89785d3f 56
JKleinRot 0:859c89785d3f 57 //Aanzetten
JKleinRot 0:859c89785d3f 58 if (rust == false && kalibratie_positie == false && kalibratie_EMG == false){
JKleinRot 0:859c89785d3f 59 lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
JKleinRot 0:859c89785d3f 60 lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
JKleinRot 0:859c89785d3f 61 wait(2); //Twee seconden wachten
JKleinRot 0:859c89785d3f 62 pc.printf("Aanzetten compleet"); //Tekst voor controle Aanzetten
JKleinRot 0:859c89785d3f 63 rust = true; //Rust wordt waar zodat door kan worden gegaan naar het volgende deel
JKleinRot 0:859c89785d3f 64 }
JKleinRot 1:e5e1eb9d0025 65
JKleinRot 1:e5e1eb9d0025 66
JKleinRot 1:e5e1eb9d0025 67
JKleinRot 1:e5e1eb9d0025 68 //Arm 1 naar thuispositie
JKleinRot 1:e5e1eb9d0025 69 if (rust == true && kalibratie_positie == false && kalibratie_EMG == false){
JKleinRot 1:e5e1eb9d0025 70 wait(1); //Een seconde wachten
JKleinRot 1:e5e1eb9d0025 71
JKleinRot 1:e5e1eb9d0025 72 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 1:e5e1eb9d0025 73
JKleinRot 2:0cb899f2800a 74 while(rust == true && kalibratie_positie == false && kalibratie_EMG == false) {
JKleinRot 1:e5e1eb9d0025 75 while(regelaar_ticker_flag != true) ;
JKleinRot 1:e5e1eb9d0025 76 regelaar_ticker_flag = false;
JKleinRot 1:e5e1eb9d0025 77
JKleinRot 1:e5e1eb9d0025 78 pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP;
JKleinRot 1:e5e1eb9d0025 79 keep_in_range(&pwm_to_motor_arm1, -1, 1);
JKleinRot 1:e5e1eb9d0025 80
JKleinRot 1:e5e1eb9d0025 81 if (pwm_to_motor_arm1 > 0){
JKleinRot 1:e5e1eb9d0025 82 dir_motor_arm1.write(1);
JKleinRot 1:e5e1eb9d0025 83 }
JKleinRot 1:e5e1eb9d0025 84 else {
JKleinRot 1:e5e1eb9d0025 85 dir_motor_arm1.write(0);
JKleinRot 1:e5e1eb9d0025 86 }
JKleinRot 1:e5e1eb9d0025 87 pwm_motor_arm1.write(abs(pwm_to_motor_arm1));
JKleinRot 2:0cb899f2800a 88
JKleinRot 2:0cb899f2800a 89 if (pwm_to_motor_arm1 == 0) {
JKleinRot 2:0cb899f2800a 90 kalibratie_positie = true;
JKleinRot 2:0cb899f2800a 91 pc.printf("Arm 1 naar thuispositie compleet");
JKleinRot 2:0cb899f2800a 92 }
JKleinRot 1:e5e1eb9d0025 93 wait(1);
JKleinRot 2:0cb899f2800a 94
JKleinRot 1:e5e1eb9d0025 95 }
JKleinRot 1:e5e1eb9d0025 96 }
JKleinRot 0:859c89785d3f 97 }