Margreeth de Breij / Mbed 2 deprecated frdm_Motor_V2_2

Definitieve motorscript in aanbouw

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_2 by Robert Schulte

main.cpp@3:687729d7996e, 2015-09-28 (annotated)

Committer:
Rvs94
Date:
Mon Sep 28 13:25:19 2015 +0000
Revision:
3:687729d7996e
Parent:
2:099da0fc31b6
Child:
4:0d4aff8b57b3
if statements toegevoegd, proberen p controller werkend te krijgen, nog niet succesvol.

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 1:48aba8d5610a 13 QEI Encoder(D3, D2, NC, 32);
Rvs94 1:48aba8d5610a 14 HIDScope scope(3);
Margreeth95 0:284ed397e046 15 Ticker ScopeTime;
Rvs94 1:48aba8d5610a 16 float Aantal_Degs;
Rvs94 1:48aba8d5610a 17 float Aantal_pulses;
Rvs94 2:099da0fc31b6 18 float Error;
Rvs94 2:099da0fc31b6 19 float refference;
Rvs94 3:687729d7996e 20 const float Kp = 0.01;
Rvs94 2:099da0fc31b6 21
Rvs94 2:099da0fc31b6 22
Margreeth95 0:284ed397e046 23 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Margreeth95 0:284ed397e046 24 {
Margreeth95 0:284ed397e046 25 scope.set(0, motor2direction.read());
Margreeth95 0:284ed397e046 26 scope.set(1, motor2speed.read());
Rvs94 1:48aba8d5610a 27 scope.set(2, Aantal_Degs);
Rvs94 1:48aba8d5610a 28 Aantal_Degs = Encoder.getPulses()*360/31/131;
Rvs94 3:687729d7996e 29 Error = refference - Aantal_Degs;
Margreeth95 0:284ed397e046 30 scope.send();
Rvs94 1:48aba8d5610a 31
Margreeth95 0:284ed397e046 32 }
Margreeth95 0:284ed397e046 33
Rvs94 3:687729d7996e 34
Margreeth95 0:284ed397e046 35 int main()
Margreeth95 0:284ed397e046 36 {
Margreeth95 0:284ed397e046 37 motor2direction = 0;
Margreeth95 0:284ed397e046 38 motor2speed = 0;
Margreeth95 0:284ed397e046 39 led = 1;
Margreeth95 0:284ed397e046 40 pc.baud(115200);
Rvs94 3:687729d7996e 41 float refference = 0;
Rvs94 3:687729d7996e 42 pc.printf("Tot aan loop werkt\n");
Margreeth95 0:284ed397e046 43 ScopeTime.attach_us(&ScopeSend, 10e4);
Margreeth95 0:284ed397e046 44
Margreeth95 0:284ed397e046 45
Margreeth95 0:284ed397e046 46 while (true)
Margreeth95 0:284ed397e046 47 {
Rvs94 3:687729d7996e 48 char c = pc.getc();
Rvs94 3:687729d7996e 49 if(c == 'r')
Rvs94 2:099da0fc31b6 50 {
Rvs94 3:687729d7996e 51 refference = refference + 10;
Rvs94 3:687729d7996e 52 pc.printf("rx \n");
Rvs94 3:687729d7996e 53 }
Rvs94 3:687729d7996e 54 if(c=='f')
Rvs94 3:687729d7996e 55 {
Rvs94 3:687729d7996e 56 refference = refference - 10;
Rvs94 3:687729d7996e 57 pc.printf("fx \n");
Rvs94 2:099da0fc31b6 58 }
Rvs94 2:099da0fc31b6 59
Rvs94 2:099da0fc31b6 60 if(Error > 0)
Rvs94 2:099da0fc31b6 61 {
Rvs94 2:099da0fc31b6 62 motor2direction = 0;
Rvs94 2:099da0fc31b6 63 }
Rvs94 2:099da0fc31b6 64 else
Rvs94 2:099da0fc31b6 65 {
Rvs94 2:099da0fc31b6 66 motor2direction = 1;
Rvs94 2:099da0fc31b6 67 }
Rvs94 2:099da0fc31b6 68 motor2speed = Kp*abs(Error);
Rvs94 3:687729d7996e 69 pc.printf("reffence = %f,error = %f \n",refference,Error);
Rvs94 3:687729d7996e 70
Margreeth95 0:284ed397e046 71 }
Margreeth95 0:284ed397e046 72 }