Margreeth de Breij / Mbed 2 deprecated frdm_MotorEMG

Motor programma met EMG

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_3 by Margreeth de Breij

main.cpp@6:3af4a77c9935, 2015-09-29 (annotated)

Committer:
Rvs94
Date:
Tue Sep 29 12:04:56 2015 +0000
Revision:
6:3af4a77c9935
Parent:
5:455773cf460b 
toevoeging van functie 'p_control' werkt niet naar behoren. Weer terug naar vorige commit.

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 6:3af4a77c9935 16 double Aantal_Degs;
Rvs94 6:3af4a77c9935 17 double Aantal_pulses;
Rvs94 6:3af4a77c9935 18 double Error;
Rvs94 6:3af4a77c9935 19 double refference;
Rvs94 2:099da0fc31b6 20
Rvs94 2:099da0fc31b6 21
Margreeth95 0:284ed397e046 22 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Margreeth95 0:284ed397e046 23 {
Margreeth95 0:284ed397e046 24 scope.set(0, motor2direction.read());
Margreeth95 0:284ed397e046 25 scope.set(1, motor2speed.read());
Rvs94 1:48aba8d5610a 26 scope.set(2, Aantal_Degs);
Rvs94 5:455773cf460b 27 Aantal_Degs = Encoder.getPulses()*360/128/131;
Rvs94 4:0d4aff8b57b3 28
Margreeth95 0:284ed397e046 29 scope.send();
Rvs94 1:48aba8d5610a 30
Margreeth95 0:284ed397e046 31 }
Rvs94 6:3af4a77c9935 32 double p_control(double kp, double Error)
Rvs94 6:3af4a77c9935 33 {
Rvs94 6:3af4a77c9935 34 return Error*kp;
Rvs94 6:3af4a77c9935 35 }
Margreeth95 0:284ed397e046 36
Rvs94 3:687729d7996e 37
Margreeth95 0:284ed397e046 38 int main()
Margreeth95 0:284ed397e046 39 {
Margreeth95 0:284ed397e046 40 motor2direction = 0;
Margreeth95 0:284ed397e046 41 motor2speed = 0;
Margreeth95 0:284ed397e046 42 led = 1;
Margreeth95 0:284ed397e046 43 pc.baud(115200);
Rvs94 6:3af4a77c9935 44 double refference = 0;
Rvs94 3:687729d7996e 45 pc.printf("Tot aan loop werkt\n");
Margreeth95 0:284ed397e046 46 ScopeTime.attach_us(&ScopeSend, 10e4);
Margreeth95 0:284ed397e046 47
Margreeth95 0:284ed397e046 48 while (true)
Margreeth95 0:284ed397e046 49 {
Rvs94 4:0d4aff8b57b3 50
Rvs94 3:687729d7996e 51 char c = pc.getc();
Rvs94 3:687729d7996e 52 if(c == 'r')
Rvs94 2:099da0fc31b6 53 {
Rvs94 3:687729d7996e 54 refference = refference + 10;
Rvs94 3:687729d7996e 55 pc.printf("rx \n");
Rvs94 4:0d4aff8b57b3 56 Error = refference - Aantal_Degs;
Rvs94 6:3af4a77c9935 57 double Output = p_control(0.05,Error);
Rvs94 6:3af4a77c9935 58
Rvs94 4:0d4aff8b57b3 59 while(abs(Error) > 2)
Rvs94 4:0d4aff8b57b3 60 {
Rvs94 4:0d4aff8b57b3 61 Error = refference - Aantal_Degs;
Rvs94 6:3af4a77c9935 62 motor2speed = abs(Output);
Rvs94 6:3af4a77c9935 63 pc.printf("reffence = %d,error = %d \n",refference,Error);
Rvs94 4:0d4aff8b57b3 64 if(Error > 0)
Rvs94 4:0d4aff8b57b3 65 {
Rvs94 4:0d4aff8b57b3 66 motor2direction = 0;
Rvs94 4:0d4aff8b57b3 67 }
Rvs94 4:0d4aff8b57b3 68 else
Rvs94 4:0d4aff8b57b3 69 {
Rvs94 5:455773cf460b 70 motor2direction = 1;
Rvs94 4:0d4aff8b57b3 71 }
Rvs94 4:0d4aff8b57b3 72
Rvs94 4:0d4aff8b57b3 73 }
Rvs94 3:687729d7996e 74 }
Rvs94 4:0d4aff8b57b3 75
Rvs94 2:099da0fc31b6 76
Rvs94 4:0d4aff8b57b3 77
Rvs94 3:687729d7996e 78
Margreeth95 0:284ed397e046 79 }
Margreeth95 0:284ed397e046 80 }