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@6:84a01494d836, 2016-10-10 (annotated)

Committer:
Marieke
Date:
Mon Oct 10 15:03:42 2016 +0000
Revision:
6:84a01494d836
Parent:
5:1b8032a15afe
Child:
7:9f4e35d977af
Motor working, HIDScope showing encoder and derivative encoder not working!

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 6:84a01494d836 34 QEI Encoder(D12, D13, NC, 32);
Marieke 6:84a01494d836 35 counts = Encoder.getPulses(); // gives position
Marieke 5:1b8032a15afe 36 scope.set(0, counts);
Marieke 5:1b8032a15afe 37 DerivativeCounts = (counts-countsPrev)/0.001;
Marieke 5:1b8032a15afe 38 scope.set(1, DerivativeCounts);
Marieke 5:1b8032a15afe 39 countsPrev = counts;
Marieke 5:1b8032a15afe 40 scope.send();
Marieke 5:1b8032a15afe 41 }
Marieke 5:1b8032a15afe 42
GerhardBerman 0:8f8157690923 43
GerhardBerman 0:8f8157690923 44 float GetReferenceVelocity()
GerhardBerman 0:8f8157690923 45 {
GerhardBerman 0:8f8157690923 46 // Returns reference velocity in rad/s.
GerhardBerman 0:8f8157690923 47 // Positive value means clockwise rotation.
Marieke 1:6be8bcde9f05 48 const float maxVelocity = 8.4; // in rad/s of course!
Marieke 2:ee821b9bf42b 49 if (button1 == 0){
Marieke 2:ee821b9bf42b 50 led1=1;
Marieke 2:ee821b9bf42b 51 led2=0;
Marieke 4:db3e61625e18 52 // Counterclockwise rotation
Marieke 2:ee821b9bf42b 53 referenceVelocity = potMeterIn * maxVelocity;
Marieke 2:ee821b9bf42b 54 }
Marieke 2:ee821b9bf42b 55 else {
Marieke 2:ee821b9bf42b 56 led1=0;
Marieke 2:ee821b9bf42b 57 led2=1;
Marieke 4:db3e61625e18 58 // Clockwise rotation
Marieke 2:ee821b9bf42b 59 referenceVelocity = -1*potMeterIn * maxVelocity;
GerhardBerman 0:8f8157690923 60 }
GerhardBerman 0:8f8157690923 61 return referenceVelocity;
GerhardBerman 0:8f8157690923 62 }
GerhardBerman 0:8f8157690923 63
GerhardBerman 0:8f8157690923 64 float FeedForwardControl(float referenceVelocity)
GerhardBerman 0:8f8157690923 65 {
GerhardBerman 0:8f8157690923 66 // very simple linear feed-forward control
GerhardBerman 0:8f8157690923 67 const float MotorGain=8.4; // unit: (rad/s) / PWM
Marieke 2:ee821b9bf42b 68 float motorValue = referenceVelocity / MotorGain;
GerhardBerman 0:8f8157690923 69 return motorValue;
GerhardBerman 0:8f8157690923 70 }
GerhardBerman 0:8f8157690923 71
GerhardBerman 0:8f8157690923 72 void SetMotor1(float motorValue)
GerhardBerman 0:8f8157690923 73 {
GerhardBerman 0:8f8157690923 74 // Given -1<=motorValue<=1, this sets the PWM and direction
GerhardBerman 0:8f8157690923 75 // bits for motor 1. Positive value makes motor rotating
GerhardBerman 0:8f8157690923 76 // clockwise. motorValues outside range are truncated to
GerhardBerman 0:8f8157690923 77 // within range
GerhardBerman 0:8f8157690923 78 if (motorValue >=0) motor1DirectionPin=1;
GerhardBerman 0:8f8157690923 79 else motor1DirectionPin=0;
GerhardBerman 0:8f8157690923 80 if (fabs(motorValue)>1) motor1MagnitudePin = 1;
GerhardBerman 0:8f8157690923 81 else motor1MagnitudePin = fabs(motorValue);
GerhardBerman 0:8f8157690923 82 }
GerhardBerman 0:8f8157690923 83
GerhardBerman 0:8f8157690923 84 void MeasureAndControl(void)
GerhardBerman 0:8f8157690923 85 {
GerhardBerman 0:8f8157690923 86 // This function measures the potmeter position, extracts a
GerhardBerman 0:8f8157690923 87 // reference velocity from it, and controls the motor with
GerhardBerman 0:8f8157690923 88 // a simple FeedForward controller. Call this from a Ticker.
Marieke 1:6be8bcde9f05 89 float referenceVelocity = GetReferenceVelocity();
GerhardBerman 0:8f8157690923 90 float motorValue = FeedForwardControl(referenceVelocity);
GerhardBerman 0:8f8157690923 91 SetMotor1(motorValue);
Marieke 6:84a01494d836 92 //pc.printf("MotorValue: %f rad/s \r\n", motorValue);
GerhardBerman 0:8f8157690923 93 }
GerhardBerman 0:8f8157690923 94
Marieke 6:84a01494d836 95 //void TimeTrackerF(){
Marieke 6:84a01494d836 96 //wait(1);
Marieke 6:84a01494d836 97 //float Potmeter = potMeterIn.read();
Marieke 6:84a01494d836 98 //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
Marieke 6:84a01494d836 99 //pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
Marieke 5:1b8032a15afe 100
Marieke 1:6be8bcde9f05 101 }
GerhardBerman 0:8f8157690923 102
GerhardBerman 0:8f8157690923 103 int main()
GerhardBerman 0:8f8157690923 104 {
GerhardBerman 0:8f8157690923 105 //Initialize
Marieke 2:ee821b9bf42b 106 led1=0;
Marieke 2:ee821b9bf42b 107 led2=0;
Marieke 5:1b8032a15afe 108 int counts;
Marieke 6:84a01494d836 109 //float Potmeter = potMeterIn.read();
Marieke 2:ee821b9bf42b 110 MeasureTicker.attach(&MeasureTicker_act, 0.01f);
Marieke 4:db3e61625e18 111 TimeTracker.attach(&TimeTracker_act, 0.3f);
Marieke 5:1b8032a15afe 112 pc.baud(115200);
Marieke 5:1b8032a15afe 113 QEI Encoder(D12, D13, NC, 32); // turns on encoder
Marieke 5:1b8032a15afe 114 sampleT.attach(&sampleT_act, 0.001f);
Marieke 6:84a01494d836 115 //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
Marieke 6:84a01494d836 116 //pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
Marieke 2:ee821b9bf42b 117
Marieke 1:6be8bcde9f05 118 while(1)
Marieke 1:6be8bcde9f05 119 {
Marieke 5:1b8032a15afe 120
Marieke 1:6be8bcde9f05 121 if (MeasureTicker_go){
Marieke 1:6be8bcde9f05 122 MeasureTicker_go=false;
Marieke 1:6be8bcde9f05 123 MeasureAndControl();
Marieke 5:1b8032a15afe 124 counts = Encoder.getPulses(); // gives position
Marieke 6:84a01494d836 125 //pc.printf("Encoder counts: %i \r\n", counts);
Marieke 1:6be8bcde9f05 126 }
Marieke 6:84a01494d836 127 /*if (TimeTracker_go){
Marieke 1:6be8bcde9f05 128 TimeTracker_go=false;
Marieke 1:6be8bcde9f05 129 TimeTrackerF();
Marieke 6:84a01494d836 130 }*/
Marieke 5:1b8032a15afe 131 if (sampleT_go){
Marieke 5:1b8032a15afe 132 sampleT_go=false;
Marieke 5:1b8032a15afe 133 sample();
Marieke 5:1b8032a15afe 134 }
Marieke 1:6be8bcde9f05 135 }
Marieke 1:6be8bcde9f05 136 }
Marieke 1:6be8bcde9f05 137
Marieke 1:6be8bcde9f05 138