Gerhard Berman / Mbed 2 deprecated prog_pract3_v2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MODSERIAL QEI mbed HIDScope

Fork of prog_pract3 by Marieke M

main.cpp@5:1b8032a15afe, 2016-10-10 (annotated)

Committer:
Marieke
Date:
Mon Oct 10 14:47:39 2016 +0000
Revision:
5:1b8032a15afe
Parent:
4:db3e61625e18
Child:
6:84a01494d836
With encoder and HIDScope integrated, USB not recognised

Who changed what in which revision?

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