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
main.cpp
- Committer:
- Rvs94
- Date:
- 2015-09-29
- Revision:
- 9:774fc3c6a39e
- Parent:
- 8:69bde5e32dbf
- Child:
- 10:80fe931a71e4
File content as of revision 9:774fc3c6a39e:
#include "mbed.h" #include "MODSERIAL.h" #include "HIDScope.h" #include "QEI.h" Serial pc(USBTX, USBRX); // tx, rx DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield) PwmOut motor2speed(D5); DigitalIn button1(SW3); DigitalIn encoderA(D3); DigitalIn encoderB(D2); AnalogIn potmeter2(A5); QEI Encoder(D3, D2, NC, 128); HIDScope scope(3); Ticker ScopeTime; Ticker myControllerTicker; double Aantal_Degs; double Aantal_pulses; void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt { scope.set(0, motor2direction.read()); scope.set(1, motor2speed.read()); scope.set(2, Aantal_Degs); Aantal_Degs = Encoder.getPulses()*360/128/131; scope.send(); } // Controller gain const double motor1_Kp = 0.05; // Reusable P controller double P( double error, const double Kp ) { return Kp * error; } // Next task, measure the error and apply the output to the plant void motor1_Controller() { double reference = potmeter2.read()*360; double position = Encoder.getPulses()*360/128/131; // Aantal Degs double P2 = P( reference - position, motor1_Kp ); motor2speed = abs(P2); if(P2 > 0) { motor2direction = 0; } else { motor2direction = 1; } } int main() { pc.baud(115200); pc.printf("Tot aan loop werkt\n"); ScopeTime.attach_us(&ScopeSend, 10e4); myControllerTicker.attach( &motor1_Controller, 0.01f ); // 100 Hz while(true) { pc.printf("position = %f aantal degs = %f \n",potmeter2.read()*360,Aantal_Degs); wait(0.1f); } }