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@7:9f4e35d977af, 2016-10-10 (annotated)

Committer:
GerhardBerman
Date:
Mon Oct 10 15:26:27 2016 +0000
Revision:
7:9f4e35d977af
Parent:
6:84a01494d836 
Replaced funcs, not yet tested

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;
GerhardBerman 7:9f4e35d977af 21 volatile int counts;
GerhardBerman 7:9f4e35d977af 22 volatile float DerivativeCounts;
GerhardBerman 7:9f4e35d977af 23 volatile 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
GerhardBerman 0:8f8157690923 31 float GetReferenceVelocity()
GerhardBerman 0:8f8157690923 32 {
GerhardBerman 0:8f8157690923 33 // Returns reference velocity in rad/s.
GerhardBerman 0:8f8157690923 34 // Positive value means clockwise rotation.
Marieke 1:6be8bcde9f05 35 const float maxVelocity = 8.4; // in rad/s of course!
Marieke 2:ee821b9bf42b 36 if (button1 == 0){
Marieke 2:ee821b9bf42b 37 led1=1;
Marieke 2:ee821b9bf42b 38 led2=0;
Marieke 4:db3e61625e18 39 // Counterclockwise rotation
Marieke 2:ee821b9bf42b 40 referenceVelocity = potMeterIn * maxVelocity;
Marieke 2:ee821b9bf42b 41 }
Marieke 2:ee821b9bf42b 42 else {
Marieke 2:ee821b9bf42b 43 led1=0;
Marieke 2:ee821b9bf42b 44 led2=1;
Marieke 4:db3e61625e18 45 // Clockwise rotation
Marieke 2:ee821b9bf42b 46 referenceVelocity = -1*potMeterIn * maxVelocity;
GerhardBerman 0:8f8157690923 47 }
GerhardBerman 0:8f8157690923 48 return referenceVelocity;
GerhardBerman 0:8f8157690923 49 }
GerhardBerman 0:8f8157690923 50
GerhardBerman 0:8f8157690923 51 float FeedForwardControl(float referenceVelocity)
GerhardBerman 0:8f8157690923 52 {
GerhardBerman 0:8f8157690923 53 // very simple linear feed-forward control
GerhardBerman 0:8f8157690923 54 const float MotorGain=8.4; // unit: (rad/s) / PWM
GerhardBerman 7:9f4e35d977af 55 volatile float motorValue = referenceVelocity / MotorGain;
GerhardBerman 0:8f8157690923 56 return motorValue;
GerhardBerman 0:8f8157690923 57 }
GerhardBerman 0:8f8157690923 58
GerhardBerman 0:8f8157690923 59 void SetMotor1(float motorValue)
GerhardBerman 0:8f8157690923 60 {
GerhardBerman 0:8f8157690923 61 // Given -1<=motorValue<=1, this sets the PWM and direction
GerhardBerman 0:8f8157690923 62 // bits for motor 1. Positive value makes motor rotating
GerhardBerman 0:8f8157690923 63 // clockwise. motorValues outside range are truncated to
GerhardBerman 0:8f8157690923 64 // within range
GerhardBerman 0:8f8157690923 65 if (motorValue >=0) motor1DirectionPin=1;
GerhardBerman 0:8f8157690923 66 else motor1DirectionPin=0;
GerhardBerman 0:8f8157690923 67 if (fabs(motorValue)>1) motor1MagnitudePin = 1;
GerhardBerman 0:8f8157690923 68 else motor1MagnitudePin = fabs(motorValue);
GerhardBerman 0:8f8157690923 69 }
GerhardBerman 0:8f8157690923 70
GerhardBerman 7:9f4e35d977af 71 void EncoderHIDScope() // encoder in HIDScope zetten
GerhardBerman 7:9f4e35d977af 72 {
GerhardBerman 7:9f4e35d977af 73 QEI Encoder(D12, D13, NC, 32);
GerhardBerman 7:9f4e35d977af 74 counts = Encoder.getPulses(); // gives position
GerhardBerman 7:9f4e35d977af 75 scope.set(0, counts);
GerhardBerman 7:9f4e35d977af 76 DerivativeCounts = (counts-countsPrev)/0.001;
GerhardBerman 7:9f4e35d977af 77 scope.set(1, DerivativeCounts);
GerhardBerman 7:9f4e35d977af 78 countsPrev = counts;
GerhardBerman 7:9f4e35d977af 79 scope.send();
GerhardBerman 7:9f4e35d977af 80 }
GerhardBerman 7:9f4e35d977af 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.
GerhardBerman 7:9f4e35d977af 87 volatile float referenceVelocity = GetReferenceVelocity();
GerhardBerman 7:9f4e35d977af 88 volatile float motorValue = FeedForwardControl(referenceVelocity);
GerhardBerman 0:8f8157690923 89 SetMotor1(motorValue);
GerhardBerman 7:9f4e35d977af 90 EncoderHIDScope();
Marieke 6:84a01494d836 91 //pc.printf("MotorValue: %f rad/s \r\n", motorValue);
GerhardBerman 0:8f8157690923 92 }
GerhardBerman 0:8f8157690923 93
GerhardBerman 7:9f4e35d977af 94 void TimeTrackerF(){
GerhardBerman 7:9f4e35d977af 95 wait(1);
GerhardBerman 7:9f4e35d977af 96 float Potmeter = potMeterIn.read();
GerhardBerman 7:9f4e35d977af 97 pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
GerhardBerman 7:9f4e35d977af 98 pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
GerhardBerman 7:9f4e35d977af 99 //pc.printf("MotorValue: %f rad/s \r\n", motorValue);
GerhardBerman 7:9f4e35d977af 100 pc.printf("Encoder counts: %i \r\n", countsPrev);
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;
GerhardBerman 7:9f4e35d977af 108 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();
GerhardBerman 7:9f4e35d977af 124 //counts = Encoder.getPulses(); // gives position
Marieke 6:84a01494d836 125 //pc.printf("Encoder counts: %i \r\n", counts);
Marieke 1:6be8bcde9f05 126 }
GerhardBerman 7:9f4e35d977af 127 if (TimeTracker_go){
Marieke 1:6be8bcde9f05 128 TimeTracker_go=false;
Marieke 1:6be8bcde9f05 129 TimeTrackerF();
GerhardBerman 7:9f4e35d977af 130 }
Marieke 5:1b8032a15afe 131 if (sampleT_go){
Marieke 5:1b8032a15afe 132 sampleT_go=false;
GerhardBerman 7:9f4e35d977af 133 EncoderHIDScope();
Marieke 5:1b8032a15afe 134 }
Marieke 1:6be8bcde9f05 135 }
Marieke 1:6be8bcde9f05 136 }