Robert Schulte / Mbed 2 deprecated frdm_Motor_V2_3

Werkend aansturingsscript voor 2 motoren, incl werkende program switch. Motoren oscilleren nog iets. Vuur mechanisme ontbreekt nog.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_2 by Margreeth de Breij

main.cpp@8:69bde5e32dbf, 2015-09-29 (annotated)

Committer:
Rvs94
Date:
Tue Sep 29 12:45:26 2015 +0000
Revision:
8:69bde5e32dbf
Parent:
7:67b50d4fb03c
Child:
9:774fc3c6a39e
reference is nu + en -. Script werkt en motor reageert goed. Enige puntje is dat de HIDScope andere waarden aangeeft dan 'aantal_degs' in putty. Next step is functies bouwen en werkend krijgen.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Margreeth95 0:284ed397e046 1 #include "mbed.h"
Margreeth95 0:284ed397e046 2 #include "MODSERIAL.h"
Margreeth95 0:284ed397e046 3 #include "HIDScope.h"
Margreeth95 0:284ed397e046 4 #include "QEI.h"
Margreeth95 0:284ed397e046 5
Margreeth95 0:284ed397e046 6 Serial pc(USBTX, USBRX); // tx, rx
Margreeth95 0:284ed397e046 7 DigitalOut led(LED_RED);
Margreeth95 0:284ed397e046 8 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
Margreeth95 0:284ed397e046 9 PwmOut motor2speed(D5);
Margreeth95 0:284ed397e046 10 DigitalIn button1(SW3);
Margreeth95 0:284ed397e046 11 DigitalIn EncoderA(D3);
Margreeth95 0:284ed397e046 12 DigitalIn EncoderB(D2);
Rvs94 5:455773cf460b 13 QEI Encoder(D3, D2, NC, 128);
Rvs94 1:48aba8d5610a 14 HIDScope scope(3);
Margreeth95 0:284ed397e046 15 Ticker ScopeTime;
Rvs94 7:67b50d4fb03c 16
Rvs94 7:67b50d4fb03c 17 double Aantal_Degs;
Rvs94 7:67b50d4fb03c 18 double Aantal_pulses;
Rvs94 7:67b50d4fb03c 19 double Error;
Rvs94 7:67b50d4fb03c 20 double refference;
Rvs94 8:69bde5e32dbf 21 const double Kp = 0.007;
Rvs94 2:099da0fc31b6 22
Rvs94 2:099da0fc31b6 23
Margreeth95 0:284ed397e046 24 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Margreeth95 0:284ed397e046 25 {
Margreeth95 0:284ed397e046 26 scope.set(0, motor2direction.read());
Margreeth95 0:284ed397e046 27 scope.set(1, motor2speed.read());
Rvs94 1:48aba8d5610a 28 scope.set(2, Aantal_Degs);
Rvs94 5:455773cf460b 29 Aantal_Degs = Encoder.getPulses()*360/128/131;
Rvs94 4:0d4aff8b57b3 30
Margreeth95 0:284ed397e046 31 scope.send();
Rvs94 1:48aba8d5610a 32
Margreeth95 0:284ed397e046 33 }
Margreeth95 0:284ed397e046 34
Rvs94 3:687729d7996e 35
Margreeth95 0:284ed397e046 36 int main()
Margreeth95 0:284ed397e046 37 {
Margreeth95 0:284ed397e046 38 motor2direction = 0;
Margreeth95 0:284ed397e046 39 motor2speed = 0;
Margreeth95 0:284ed397e046 40 led = 1;
Margreeth95 0:284ed397e046 41 pc.baud(115200);
Rvs94 7:67b50d4fb03c 42 refference = 0;
Rvs94 3:687729d7996e 43 pc.printf("Tot aan loop werkt\n");
Margreeth95 0:284ed397e046 44 ScopeTime.attach_us(&ScopeSend, 10e4);
Margreeth95 0:284ed397e046 45
Margreeth95 0:284ed397e046 46
Margreeth95 0:284ed397e046 47 while (true)
Margreeth95 0:284ed397e046 48 {
Rvs94 4:0d4aff8b57b3 49
Rvs94 3:687729d7996e 50 char c = pc.getc();
Rvs94 3:687729d7996e 51 if(c == 'r')
Rvs94 2:099da0fc31b6 52 {
Rvs94 3:687729d7996e 53 refference = refference + 10;
Rvs94 3:687729d7996e 54 pc.printf("rx \n");
Rvs94 4:0d4aff8b57b3 55 Error = refference - Aantal_Degs;
Rvs94 7:67b50d4fb03c 56 while(abs(Error) > 1)
Rvs94 4:0d4aff8b57b3 57 {
Rvs94 4:0d4aff8b57b3 58 Error = refference - Aantal_Degs;
Rvs94 4:0d4aff8b57b3 59 motor2speed = Kp*abs(Error);
Rvs94 7:67b50d4fb03c 60 pc.printf("reffence = %f,error = %f,Aantal degs = %f \n",refference,Error,Aantal_Degs);
Rvs94 4:0d4aff8b57b3 61 if(Error > 0)
Rvs94 4:0d4aff8b57b3 62 {
Rvs94 4:0d4aff8b57b3 63 motor2direction = 0;
Rvs94 4:0d4aff8b57b3 64 }
Rvs94 4:0d4aff8b57b3 65 else
Rvs94 4:0d4aff8b57b3 66 {
Rvs94 5:455773cf460b 67 motor2direction = 1;
Rvs94 4:0d4aff8b57b3 68 }
Rvs94 4:0d4aff8b57b3 69 }
Rvs94 3:687729d7996e 70 }
Rvs94 8:69bde5e32dbf 71 if(c == 'f')
Rvs94 8:69bde5e32dbf 72 {
Rvs94 8:69bde5e32dbf 73 refference = refference - 10;
Rvs94 8:69bde5e32dbf 74 pc.printf("rx \n");
Rvs94 8:69bde5e32dbf 75 Error = refference - Aantal_Degs;
Rvs94 8:69bde5e32dbf 76 while(abs(Error) > 1)
Rvs94 8:69bde5e32dbf 77 {
Rvs94 8:69bde5e32dbf 78 Error = refference - Aantal_Degs;
Rvs94 8:69bde5e32dbf 79 motor2speed = Kp*abs(Error);
Rvs94 8:69bde5e32dbf 80 pc.printf("reffence = %f,error = %f,Aantal degs = %f \n",refference,Error,Aantal_Degs);
Rvs94 8:69bde5e32dbf 81 if(Error > 0)
Rvs94 8:69bde5e32dbf 82 {
Rvs94 8:69bde5e32dbf 83 motor2direction = 0;
Rvs94 8:69bde5e32dbf 84 }
Rvs94 8:69bde5e32dbf 85 else
Rvs94 8:69bde5e32dbf 86 {
Rvs94 8:69bde5e32dbf 87 motor2direction = 1;
Rvs94 8:69bde5e32dbf 88 }
Rvs94 8:69bde5e32dbf 89 }
Rvs94 8:69bde5e32dbf 90 }
Margreeth95 0:284ed397e046 91 }
Margreeth95 0:284ed397e046 92 }