Marieke M / Mbed 2 deprecated prog_practPutty

Counts and Derivative of counts working in Putty, first changes for HIDScope made.

Dependencies:   MODSERIAL QEI mbed HIDScope

main.cpp@2:915abd97ac16, 2016-10-11 (annotated)

Committer:
Marieke
Date:
Tue Oct 11 11:38:43 2016 +0000
Revision:
2:915abd97ac16
Parent:
1:812a1637b6cb 
HIDScope all working!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Marieke 0:818fc79663f2 1 #include "mbed.h"
Marieke 0:818fc79663f2 2 #include <math.h>
Marieke 0:818fc79663f2 3 #include "MODSERIAL.h"
Marieke 0:818fc79663f2 4 #include "QEI.h"
Marieke 2:915abd97ac16 5 #include "HIDScope.h"
Marieke 0:818fc79663f2 6
Marieke 0:818fc79663f2 7 DigitalIn encoder1A (D13); //Channel A van Encoder 1
Marieke 0:818fc79663f2 8 DigitalIn encoder1B (D12); //Channel B van Encoder 1
Marieke 0:818fc79663f2 9 DigitalOut led1 (D11);
Marieke 0:818fc79663f2 10 DigitalOut led2 (D10);
Marieke 0:818fc79663f2 11 AnalogIn potMeterIn(A0);
Marieke 0:818fc79663f2 12 DigitalOut motor1DirectionPin(D7);
Marieke 0:818fc79663f2 13 PwmOut motor1MagnitudePin(D6);
Marieke 0:818fc79663f2 14 DigitalIn button1(D5);
Marieke 0:818fc79663f2 15
Marieke 0:818fc79663f2 16
Marieke 0:818fc79663f2 17 Serial pc(USBTX,USBRX);
Marieke 2:915abd97ac16 18 Ticker MeasureTicker;// sampleT, TimeTracker;
Marieke 2:915abd97ac16 19 HIDScope scope(2);
Marieke 0:818fc79663f2 20 int counts;
Marieke 2:915abd97ac16 21 double DerivativeCounts;
Marieke 0:818fc79663f2 22 int countsPrev = 0;
Marieke 0:818fc79663f2 23
Marieke 0:818fc79663f2 24 float referenceVelocity = 0;
Marieke 0:818fc79663f2 25
Marieke 2:915abd97ac16 26 volatile bool MeasureTicker_go=false;// TimeTracker_go=false, sampleT_go=false;
Marieke 0:818fc79663f2 27
Marieke 0:818fc79663f2 28 void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
Marieke 2:915abd97ac16 29 //void TimeTracker_act(){TimeTracker_go=true;};
Marieke 2:915abd97ac16 30 //void sampleT_act(){sampleT_go=true;};
Marieke 0:818fc79663f2 31
Marieke 0:818fc79663f2 32 float GetReferenceVelocity()
Marieke 0:818fc79663f2 33 {
Marieke 0:818fc79663f2 34 // Returns reference velocity in rad/s.
Marieke 0:818fc79663f2 35 // Positive value means clockwise rotation.
Marieke 0:818fc79663f2 36 const float maxVelocity = 8.4; // in rad/s of course!
Marieke 0:818fc79663f2 37 if (button1 == 0){
Marieke 0:818fc79663f2 38 led1=1;
Marieke 0:818fc79663f2 39 led2=0;
Marieke 0:818fc79663f2 40 // Counterclockwise rotation
Marieke 0:818fc79663f2 41 referenceVelocity = potMeterIn * maxVelocity;
Marieke 0:818fc79663f2 42 }
Marieke 0:818fc79663f2 43 else {
Marieke 0:818fc79663f2 44 led1=0;
Marieke 0:818fc79663f2 45 led2=1;
Marieke 0:818fc79663f2 46 // Clockwise rotation
Marieke 0:818fc79663f2 47 referenceVelocity = -1*potMeterIn * maxVelocity;
Marieke 0:818fc79663f2 48 }
Marieke 0:818fc79663f2 49 return referenceVelocity;
Marieke 0:818fc79663f2 50 }
Marieke 0:818fc79663f2 51
Marieke 0:818fc79663f2 52 float FeedForwardControl(float referenceVelocity)
Marieke 0:818fc79663f2 53 {
Marieke 0:818fc79663f2 54 // very simple linear feed-forward control
Marieke 0:818fc79663f2 55 const float MotorGain=8.4; // unit: (rad/s) / PWM
Marieke 0:818fc79663f2 56 float motorValue = referenceVelocity / MotorGain;
Marieke 0:818fc79663f2 57 return motorValue;
Marieke 0:818fc79663f2 58 }
Marieke 0:818fc79663f2 59
Marieke 0:818fc79663f2 60 void SetMotor1(float motorValue)
Marieke 0:818fc79663f2 61 {
Marieke 0:818fc79663f2 62 // Given -1<=motorValue<=1, this sets the PWM and direction
Marieke 0:818fc79663f2 63 // bits for motor 1. Positive value makes motor rotating
Marieke 0:818fc79663f2 64 // clockwise. motorValues outside range are truncated to
Marieke 0:818fc79663f2 65 // within range
Marieke 0:818fc79663f2 66 if (motorValue >=0) motor1DirectionPin=1;
Marieke 0:818fc79663f2 67 else motor1DirectionPin=0;
Marieke 0:818fc79663f2 68 if (fabs(motorValue)>1) motor1MagnitudePin = 1;
Marieke 0:818fc79663f2 69 else motor1MagnitudePin = fabs(motorValue);
Marieke 0:818fc79663f2 70 }
Marieke 0:818fc79663f2 71
Marieke 0:818fc79663f2 72 void MeasureAndControl()
Marieke 0:818fc79663f2 73 {
Marieke 0:818fc79663f2 74 // This function measures the potmeter position, extracts a
Marieke 0:818fc79663f2 75 // reference velocity from it, and controls the motor with
Marieke 0:818fc79663f2 76 // a simple FeedForward controller. Call this from a Ticker.
Marieke 0:818fc79663f2 77 float referenceVelocity = GetReferenceVelocity();
Marieke 0:818fc79663f2 78 float motorValue = FeedForwardControl(referenceVelocity);
Marieke 0:818fc79663f2 79 SetMotor1(motorValue);
Marieke 0:818fc79663f2 80 }
Marieke 0:818fc79663f2 81
Marieke 0:818fc79663f2 82 void TimeTrackerF(){
Marieke 0:818fc79663f2 83 wait(1);
Marieke 0:818fc79663f2 84 float Potmeter = potMeterIn.read();
Marieke 0:818fc79663f2 85 pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
Marieke 0:818fc79663f2 86 pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
Marieke 0:818fc79663f2 87 //pc.printf("Counts: %i rad/s \r\n", counts);
Marieke 0:818fc79663f2 88 //pc.printf("Derivative Counts: %i rad/s \r\n", DerivativeCounts);
Marieke 0:818fc79663f2 89 }
Marieke 0:818fc79663f2 90 /*
Marieke 0:818fc79663f2 91 void sample()
Marieke 0:818fc79663f2 92 {
Marieke 0:818fc79663f2 93 int countsPrev = 0;
Marieke 0:818fc79663f2 94 QEI Encoder(D12, D13, NC, 32);
Marieke 0:818fc79663f2 95 counts = Encoder.getPulses(); // gives position
Marieke 0:818fc79663f2 96 //scope.set(0,counts);
Marieke 0:818fc79663f2 97 DerivativeCounts = (counts-countsPrev)/0.001;
Marieke 0:818fc79663f2 98 //scope.set(1,DerivativeCounts);
Marieke 0:818fc79663f2 99 countsPrev = counts;
Marieke 0:818fc79663f2 100 //scope.send();
Marieke 0:818fc79663f2 101 pc.printf("Counts: %i rad/s \r\n", counts);
Marieke 0:818fc79663f2 102 pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts);
Marieke 0:818fc79663f2 103 }*/
Marieke 0:818fc79663f2 104
Marieke 0:818fc79663f2 105 int main()
Marieke 0:818fc79663f2 106 {
Marieke 0:818fc79663f2 107 //Initialize
Marieke 0:818fc79663f2 108 led1=0;
Marieke 0:818fc79663f2 109 led2=0;
Marieke 0:818fc79663f2 110 float Potmeter = potMeterIn.read();
Marieke 0:818fc79663f2 111 MeasureTicker.attach(&MeasureTicker_act, 0.01f);
Marieke 2:915abd97ac16 112 //TimeTracker.attach(&TimeTracker_act, 0.1f);
Marieke 0:818fc79663f2 113 pc.baud(115200);
Marieke 0:818fc79663f2 114 QEI Encoder(D12, D13, NC, 32); // turns on encoder
Marieke 0:818fc79663f2 115 //sampleT.attach(&sampleT_act, 0.1f);
Marieke 2:915abd97ac16 116 //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
Marieke 2:915abd97ac16 117 //pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
Marieke 0:818fc79663f2 118
Marieke 0:818fc79663f2 119 while(1)
Marieke 0:818fc79663f2 120 {
Marieke 0:818fc79663f2 121 if (MeasureTicker_go){
Marieke 0:818fc79663f2 122 MeasureTicker_go=false;
Marieke 0:818fc79663f2 123 MeasureAndControl();
Marieke 0:818fc79663f2 124 // Encoder part
Marieke 0:818fc79663f2 125 counts = Encoder.getPulses(); // gives position
Marieke 2:915abd97ac16 126 DerivativeCounts = ((double) counts-countsPrev)/0.01;
Marieke 1:812a1637b6cb 127 countsPrev = counts;
Marieke 1:812a1637b6cb 128 scope.set(0,counts);
Marieke 1:812a1637b6cb 129 scope.set(1,DerivativeCounts);
Marieke 1:812a1637b6cb 130 scope.send();countsPrev = counts;
Marieke 2:915abd97ac16 131 //pc.printf("Counts: %i rad/s \r\n", counts);
Marieke 2:915abd97ac16 132 //pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts);
Marieke 0:818fc79663f2 133 }
Marieke 2:915abd97ac16 134 /*if (TimeTracker_go){
Marieke 0:818fc79663f2 135 TimeTracker_go=false;
Marieke 0:818fc79663f2 136 TimeTrackerF();
Marieke 0:818fc79663f2 137 }
Marieke 2:915abd97ac16 138 if (sampleT_go){
Marieke 0:818fc79663f2 139 sampleT_go=false;
Marieke 0:818fc79663f2 140 sample();
Marieke 0:818fc79663f2 141 }*/
Marieke 0:818fc79663f2 142 }
Marieke 0:818fc79663f2 143 }