Putty working, giving: Potmeter value, reference velocity and Motorvalue=0! Motor only works on 2.
Dependencies: MODSERIAL mbed HIDScope QEI
Fork of prog_pract3 by
main.cpp
- Committer:
- Marieke
- Date:
- 2016-10-10
- Revision:
- 6:84a01494d836
- Parent:
- 5:1b8032a15afe
File content as of revision 6:84a01494d836:
#include "mbed.h" #include <math.h> #include "MODSERIAL.h" #include "QEI.h" #include "HIDScope.h" DigitalIn encoder1A (D13); //Channel A van Encoder 1 DigitalIn encoder1B (D12); //Channel B van Encoder 1 DigitalOut led1 (D11); DigitalOut led2 (D10); AnalogIn potMeterIn(PTB2); DigitalIn button1(D5); DigitalOut motor1DirectionPin(D7); PwmOut motor1MagnitudePin(D6); Serial pc(USBTX,USBRX); Ticker MeasureTicker, TimeTracker, sampleT; HIDScope scope(2); float referenceVelocity = 0; int counts; double DerivativeCounts; int countsPrev = 0; volatile bool MeasureTicker_go=false, TimeTracker_go=false, sampleT_go=false; void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags void TimeTracker_act(){TimeTracker_go=true;}; void sampleT_act(){sampleT_go=true;}; // encoder in HIDScope setten void sample() { QEI Encoder(D12, D13, NC, 32); counts = Encoder.getPulses(); // gives position scope.set(0, counts); DerivativeCounts = (counts-countsPrev)/0.001; scope.set(1, DerivativeCounts); countsPrev = counts; scope.send(); } float GetReferenceVelocity() { // Returns reference velocity in rad/s. // Positive value means clockwise rotation. const float maxVelocity = 8.4; // in rad/s of course! if (button1 == 0){ led1=1; led2=0; // Counterclockwise rotation referenceVelocity = potMeterIn * maxVelocity; } else { led1=0; led2=1; // Clockwise rotation referenceVelocity = -1*potMeterIn * maxVelocity; } return referenceVelocity; } float FeedForwardControl(float referenceVelocity) { // very simple linear feed-forward control const float MotorGain=8.4; // unit: (rad/s) / PWM float motorValue = referenceVelocity / MotorGain; return motorValue; } void SetMotor1(float motorValue) { // Given -1<=motorValue<=1, this sets the PWM and direction // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range if (motorValue >=0) motor1DirectionPin=1; else motor1DirectionPin=0; if (fabs(motorValue)>1) motor1MagnitudePin = 1; else motor1MagnitudePin = fabs(motorValue); } void MeasureAndControl(void) { // This function measures the potmeter position, extracts a // reference velocity from it, and controls the motor with // a simple FeedForward controller. Call this from a Ticker. float referenceVelocity = GetReferenceVelocity(); float motorValue = FeedForwardControl(referenceVelocity); SetMotor1(motorValue); //pc.printf("MotorValue: %f rad/s \r\n", motorValue); } //void TimeTrackerF(){ //wait(1); //float Potmeter = potMeterIn.read(); //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity); //pc.printf("Potmeter: %f rad/s \r\n", Potmeter); } int main() { //Initialize led1=0; led2=0; int counts; //float Potmeter = potMeterIn.read(); MeasureTicker.attach(&MeasureTicker_act, 0.01f); TimeTracker.attach(&TimeTracker_act, 0.3f); pc.baud(115200); QEI Encoder(D12, D13, NC, 32); // turns on encoder sampleT.attach(&sampleT_act, 0.001f); //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity); //pc.printf("Potmeter: %f rad/s \r\n", Potmeter); while(1) { if (MeasureTicker_go){ MeasureTicker_go=false; MeasureAndControl(); counts = Encoder.getPulses(); // gives position //pc.printf("Encoder counts: %i \r\n", counts); } /*if (TimeTracker_go){ TimeTracker_go=false; TimeTrackerF(); }*/ if (sampleT_go){ sampleT_go=false; sample(); } } }