Linde Haverkate / Mbed 2 deprecated template_biorobotics1

potmeter motor ffwd

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL Encoder FastPWM

main.cpp@2:6ca30ccec353, 2019-10-28 (annotated)

Committer:
linde101
Date:
Mon Oct 28 16:31:37 2019 +0000
Revision:
2:6ca30ccec353
Parent:
1:b862262a9d14 
Potentiometer aangestuurde motoren feed-forward;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
linde101 2:6ca30ccec353 2 #include "math.h"
linde101 2:6ca30ccec353 3 #include "encoder.h"
linde101 2:6ca30ccec353 4 #include "QEI.h"
RobertoO 0:67c50348f842 5 //#include "HIDScope.h"
linde101 2:6ca30ccec353 6 // ---- Alles met een 1(of geen getal) gaat over motor 1----
linde101 2:6ca30ccec353 7 // ---- Alles met een 2 gaat over motor 2----
linde101 2:6ca30ccec353 8 // ------ Hardware Interfaces -------
linde101 2:6ca30ccec353 9
linde101 2:6ca30ccec353 10 const PinName motor1dir = D7; //direction1 (cw,ccw)
linde101 2:6ca30ccec353 11 const PinName motor1PWM = D6; //speed1
linde101 2:6ca30ccec353 12 const PinName motor2PWM = D5; //speed2
linde101 2:6ca30ccec353 13 const PinName motor2dir = D4; //direction2 (cw,ccw)
linde101 2:6ca30ccec353 14
linde101 2:6ca30ccec353 15 DigitalOut motor1direction(motor1dir);
linde101 2:6ca30ccec353 16 PwmOut motor1control(motor1PWM);
linde101 2:6ca30ccec353 17 PwmOut motor2control(motor2PWM);
linde101 2:6ca30ccec353 18 DigitalOut motor2direction(motor2dir);
linde101 2:6ca30ccec353 19
linde101 2:6ca30ccec353 20 const PinName button1name = D10;
linde101 2:6ca30ccec353 21 const PinName pot1name = A0;
linde101 2:6ca30ccec353 22 const PinName button2name = D11;
linde101 2:6ca30ccec353 23 const PinName pot2name = A1;
linde101 2:6ca30ccec353 24 InterruptIn button1(button1name); //had ook direct gekund maar he
linde101 2:6ca30ccec353 25 AnalogIn potMeterIn1(pot1name);
linde101 2:6ca30ccec353 26 InterruptIn button2(button2name);
linde101 2:6ca30ccec353 27 AnalogIn potMeterIn2(pot2name);
RobertoO 0:67c50348f842 28
linde101 2:6ca30ccec353 29 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //64 = ..., x4 encoding because ...
linde101 2:6ca30ccec353 30 QEI Encoder2(D9,D8,NC,64,QEI::X4_ENCODING);
linde101 2:6ca30ccec353 31 // ------- Objects used -------
linde101 2:6ca30ccec353 32
linde101 2:6ca30ccec353 33 // Ticker which controls the mother every 1/100 of a second.
linde101 2:6ca30ccec353 34 Ticker controlTicker;
linde101 2:6ca30ccec353 35
linde101 2:6ca30ccec353 36 Ticker debugTicker;
linde101 2:6ca30ccec353 37 Serial pc(USBTX, USBRX);
linde101 2:6ca30ccec353 38
linde101 2:6ca30ccec353 39 //HIDScope scope(2); //fuck you hidscope we're not using you!
linde101 2:6ca30ccec353 40
linde101 2:6ca30ccec353 41
linde101 2:6ca30ccec353 42 // ------- Constants
linde101 2:6ca30ccec353 43 const float motorGain = 8f; //just to be sure, niet te veel input geven tegen windup (old motor, suboptimal perfomance)
linde101 2:6ca30ccec353 44 const float maxVelocity = 8f; //radians per second
linde101 2:6ca30ccec353 45 const bool clockwise = true;
linde101 2:6ca30ccec353 46 const float rad_count = 0.0007479; // 2pi/8400;
linde101 2:6ca30ccec353 47 const float pi = 3.1415926;
linde101 2:6ca30ccec353 48
linde101 2:6ca30ccec353 49 // ------ variables
linde101 2:6ca30ccec353 50
linde101 2:6ca30ccec353 51 volatile bool direction1 = clockwise;
linde101 2:6ca30ccec353 52 volatile bool direction2 = clockwise;
linde101 2:6ca30ccec353 53
linde101 2:6ca30ccec353 54 // ------ functions
linde101 2:6ca30ccec353 55
linde101 2:6ca30ccec353 56 float getReferenceVelocity() {
linde101 2:6ca30ccec353 57 // Returns reference velocity in rad/s.
linde101 2:6ca30ccec353 58 return maxVelocity * potMeterIn1 ;
linde101 2:6ca30ccec353 59 }
linde101 2:6ca30ccec353 60 float getReferenceVelocity2() {
linde101 2:6ca30ccec353 61 // Returns reference velocity in rad/s.
linde101 2:6ca30ccec353 62 return maxVelocity * potMeterIn2;
linde101 2:6ca30ccec353 63 }
linde101 2:6ca30ccec353 64
linde101 2:6ca30ccec353 65 void setMotor1(float motorValue1) {
linde101 2:6ca30ccec353 66 // Given motorValue1<=1, writes the velocity to the pwm control.
linde101 2:6ca30ccec353 67 // MotorValues outside range are truncated to within range.
linde101 2:6ca30ccec353 68 motor1control.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
linde101 2:6ca30ccec353 69 }
linde101 2:6ca30ccec353 70
linde101 2:6ca30ccec353 71 void setMotor2(float motorValue2) {
linde101 2:6ca30ccec353 72 // Given motorValue2<=1, writes the velocity to the pwm control.
linde101 2:6ca30ccec353 73 // MotorValues outside range are truncated to within range.
linde101 2:6ca30ccec353 74 motor2control.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
linde101 2:6ca30ccec353 75 }
linde101 2:6ca30ccec353 76
linde101 2:6ca30ccec353 77 float feedForwardControl(float referenceVelocity) {
linde101 2:6ca30ccec353 78 // very simple linear feed-forward control
linde101 2:6ca30ccec353 79 // returns motorValue
linde101 2:6ca30ccec353 80 return referenceVelocity / motorGain;
linde101 2:6ca30ccec353 81 }
linde101 2:6ca30ccec353 82
linde101 2:6ca30ccec353 83 float feedForwardControl2(float referenceVelocity2) {
linde101 2:6ca30ccec353 84 // very simple linear feed-forward control
linde101 2:6ca30ccec353 85 // returns motorValue
linde101 2:6ca30ccec353 86 return referenceVelocity2 / motorGain;
linde101 2:6ca30ccec353 87 }
linde101 2:6ca30ccec353 88
linde101 2:6ca30ccec353 89 void measureAndControl(void) {
linde101 2:6ca30ccec353 90 // This function measures the potmeter position, extracts a
linde101 2:6ca30ccec353 91 // reference velocity from it, and controls the motor with
linde101 2:6ca30ccec353 92 // a simple FeedForward controller. Call this from a Ticker.
linde101 2:6ca30ccec353 93 float referenceVelocity = getReferenceVelocity();
linde101 2:6ca30ccec353 94 float motorValue1 = feedForwardControl(referenceVelocity);
linde101 2:6ca30ccec353 95 setMotor1(motorValue1);
linde101 2:6ca30ccec353 96
linde101 2:6ca30ccec353 97 float referenceVelocity2 = getReferenceVelocity2();
linde101 2:6ca30ccec353 98 float motorValue2 = feedForwardControl2(referenceVelocity2);
linde101 2:6ca30ccec353 99 setMotor2(motorValue2);
linde101 2:6ca30ccec353 100 }
linde101 2:6ca30ccec353 101
linde101 2:6ca30ccec353 102 void onButtonPress() {
linde101 2:6ca30ccec353 103 // reverses the direction
linde101 2:6ca30ccec353 104 motor1direction.write(direction1 = !direction1);
linde101 2:6ca30ccec353 105 pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
linde101 2:6ca30ccec353 106 }
linde101 2:6ca30ccec353 107
linde101 2:6ca30ccec353 108 void onButtonPress2() {
linde101 2:6ca30ccec353 109 // reverses the direction
linde101 2:6ca30ccec353 110 motor2direction.write(direction2 = !direction2);
linde101 2:6ca30ccec353 111 pc.printf("direction: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
linde101 2:6ca30ccec353 112 }
RobertoO 0:67c50348f842 113
linde101 2:6ca30ccec353 114 float Encoding()
linde101 2:6ca30ccec353 115 {
linde101 2:6ca30ccec353 116 // Encoding is necessary for the computations of the joint angles and
linde101 2:6ca30ccec353 117 // velocities of the arm given certain linear velocities.
linde101 2:6ca30ccec353 118
linde101 2:6ca30ccec353 119 double counts1 = Encoder1.getPulses();
linde101 2:6ca30ccec353 120 double rad_m1 = (rad_count * (double)counts1) + (1.0f*pi/3.0f);
linde101 2:6ca30ccec353 121 return rad_m1;
linde101 2:6ca30ccec353 122 }
linde101 2:6ca30ccec353 123
linde101 2:6ca30ccec353 124 float Encoding2()
linde101 2:6ca30ccec353 125 {
linde101 2:6ca30ccec353 126 // Encoding is necessary for the computations of the joint angles and
linde101 2:6ca30ccec353 127 // velocities of the arm given certain linear velocities.
linde101 2:6ca30ccec353 128
linde101 2:6ca30ccec353 129 double counts2 = Encoder2.getPulses();
linde101 2:6ca30ccec353 130 double rad_m2 = (rad_count * (double)counts2) + (7.0f*pi/4.0f);
linde101 2:6ca30ccec353 131 return rad_m2;
linde101 2:6ca30ccec353 132 }
RobertoO 0:67c50348f842 133
linde101 2:6ca30ccec353 134 void onDebugTick() {
linde101 2:6ca30ccec353 135
linde101 2:6ca30ccec353 136 pc.printf("pot input1: %f\r\n", potMeterIn1.read());
linde101 2:6ca30ccec353 137 pc.printf("motorValue1: %f\r\n", feedForwardControl(getReferenceVelocity()));
linde101 2:6ca30ccec353 138 pc.printf("radialen motor 1: %f\r\n",Encoding());
linde101 2:6ca30ccec353 139
linde101 2:6ca30ccec353 140 pc.printf("\n\n\n");
linde101 2:6ca30ccec353 141
linde101 2:6ca30ccec353 142 /*
linde101 2:6ca30ccec353 143 scope.set(0, potMeterIn.read());
linde101 2:6ca30ccec353 144 scope.set(1, feedForwardControl(getReferenceVelocity()));
linde101 2:6ca30ccec353 145 scope.send();
linde101 2:6ca30ccec353 146 */
linde101 2:6ca30ccec353 147 pc.printf("pot input2: %f\r\n", potMeterIn2.read());
linde101 2:6ca30ccec353 148 pc.printf("motorValue2: %f\r\n", feedForwardControl2(getReferenceVelocity2()));
linde101 2:6ca30ccec353 149 pc.printf("radialen motor 2: %f\r\n",Encoding2());
linde101 2:6ca30ccec353 150 pc.printf("\n\n\n");
linde101 2:6ca30ccec353 151 }
linde101 2:6ca30ccec353 152
RobertoO 0:67c50348f842 153 int main()
RobertoO 0:67c50348f842 154 {
RobertoO 0:67c50348f842 155 pc.baud(115200);
RobertoO 0:67c50348f842 156
linde101 2:6ca30ccec353 157 button2.fall(&onButtonPress2);
linde101 2:6ca30ccec353 158 controlTicker.attach(&measureAndControl, 1.0f/100.0f);
linde101 2:6ca30ccec353 159 debugTicker.attach(&onDebugTick, 1);
linde101 2:6ca30ccec353 160
linde101 2:6ca30ccec353 161 button1.fall(&onButtonPress);
linde101 2:6ca30ccec353 162
linde101 2:6ca30ccec353 163 }