![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
first itteration
Dependencies: MODSERIAL QEI mbed
main.cpp@6:1a410879a539, 2017-10-27 (annotated)
- Committer:
- Arnoud113
- Date:
- Fri Oct 27 09:38:09 2017 +0000
- Revision:
- 6:1a410879a539
- Parent:
- 5:a3848a66a4df
- Child:
- 7:080805fc3cf0
working velocity and encoder control;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Arnoud113 | 0:1816743ba8ba | 1 | #include "mbed.h" |
Arnoud113 | 0:1816743ba8ba | 2 | #include "QEI.h" |
Arnoud113 | 0:1816743ba8ba | 3 | #include "MODSERIAL.h" |
Arnoud113 | 0:1816743ba8ba | 4 | #include "math.h" |
Arnoud113 | 0:1816743ba8ba | 5 | |
Arnoud113 | 0:1816743ba8ba | 6 | |
Arnoud113 | 0:1816743ba8ba | 7 | |
Arnoud113 | 0:1816743ba8ba | 8 | DigitalOut gpo(D0); |
Arnoud113 | 3:cc3766838777 | 9 | DigitalOut ledb(LED_BLUE); |
Arnoud113 | 3:cc3766838777 | 10 | DigitalOut ledr(LED_RED); |
Arnoud113 | 3:cc3766838777 | 11 | DigitalOut ledg(LED_GREEN); |
Arnoud113 | 1:659489c32e75 | 12 | DigitalOut motor1DC(D7); |
Arnoud113 | 2:abd40da4a5e2 | 13 | PwmOut motor1PWM(D6); |
Arnoud113 | 0:1816743ba8ba | 14 | DigitalOut motor2DC(D4); |
Arnoud113 | 2:abd40da4a5e2 | 15 | PwmOut motor2PWM(D5); |
Arnoud113 | 0:1816743ba8ba | 16 | |
Arnoud113 | 3:cc3766838777 | 17 | AnalogIn potMeterIn1(A0); |
Arnoud113 | 4:983b50758735 | 18 | AnalogIn potMeterIn2(A1); |
Arnoud113 | 3:cc3766838777 | 19 | |
Arnoud113 | 0:1816743ba8ba | 20 | DigitalIn button1(D11); |
Arnoud113 | 0:1816743ba8ba | 21 | |
Arnoud113 | 0:1816743ba8ba | 22 | MODSERIAL pc(USBTX,USBRX); |
Arnoud113 | 4:983b50758735 | 23 | QEI Encoder(D12,D13,NC,32); |
Arnoud113 | 3:cc3766838777 | 24 | Ticker controller; |
Arnoud113 | 1:659489c32e75 | 25 | |
Arnoud113 | 0:1816743ba8ba | 26 | |
Arnoud113 | 5:a3848a66a4df | 27 | |
Arnoud113 | 5:a3848a66a4df | 28 | float GetReferencePosition() |
Arnoud113 | 5:a3848a66a4df | 29 | { |
Arnoud113 | 6:1a410879a539 | 30 | int potmultiplier = 8000; // constant to multiply the pot 2 value with to get a reference position |
Arnoud113 | 5:a3848a66a4df | 31 | |
Arnoud113 | 5:a3848a66a4df | 32 | float referencePosition; |
Arnoud113 | 5:a3848a66a4df | 33 | referencePosition = potMeterIn2 * potmultiplier; |
Arnoud113 | 5:a3848a66a4df | 34 | return referencePosition; |
Arnoud113 | 5:a3848a66a4df | 35 | } |
Arnoud113 | 5:a3848a66a4df | 36 | |
Arnoud113 | 0:1816743ba8ba | 37 | float GetReferenceVelocity() |
Arnoud113 | 0:1816743ba8ba | 38 | { |
Arnoud113 | 0:1816743ba8ba | 39 | // Returns reference velocity in rad/s. |
Arnoud113 | 0:1816743ba8ba | 40 | // Positive value means clockwise rotation. |
Arnoud113 | 0:1816743ba8ba | 41 | |
Arnoud113 | 6:1a410879a539 | 42 | int referencePosition = GetReferencePosition(); |
Arnoud113 | 6:1a410879a539 | 43 | int counts = Encoder.getPulses(); |
Arnoud113 | 6:1a410879a539 | 44 | |
Arnoud113 | 0:1816743ba8ba | 45 | const float maxVelocity=8.4; // in rad/s of course! |
Arnoud113 | 0:1816743ba8ba | 46 | |
Arnoud113 | 0:1816743ba8ba | 47 | float referenceVelocity; // in rad/s |
Arnoud113 | 0:1816743ba8ba | 48 | |
Arnoud113 | 6:1a410879a539 | 49 | if (counts > referencePosition) { |
Arnoud113 | 0:1816743ba8ba | 50 | // Clockwise rotation |
Arnoud113 | 6:1a410879a539 | 51 | referenceVelocity = -1*potMeterIn1 * maxVelocity; |
Arnoud113 | 3:cc3766838777 | 52 | ledr = 1; |
Arnoud113 | 6:1a410879a539 | 53 | ledg = 1; |
Arnoud113 | 3:cc3766838777 | 54 | ledb = 0; |
Arnoud113 | 0:1816743ba8ba | 55 | } |
Arnoud113 | 6:1a410879a539 | 56 | else if ( counts < referencePosition){ |
Arnoud113 | 0:1816743ba8ba | 57 | // Counterclockwise rotation |
Arnoud113 | 6:1a410879a539 | 58 | referenceVelocity = potMeterIn1 * maxVelocity; |
Arnoud113 | 3:cc3766838777 | 59 | ledb = 1; |
Arnoud113 | 6:1a410879a539 | 60 | ledg = 1; |
Arnoud113 | 3:cc3766838777 | 61 | ledr = 0; |
Arnoud113 | 0:1816743ba8ba | 62 | } |
Arnoud113 | 6:1a410879a539 | 63 | else{ |
Arnoud113 | 6:1a410879a539 | 64 | referenceVelocity = 0; |
Arnoud113 | 6:1a410879a539 | 65 | ledb = 1; |
Arnoud113 | 6:1a410879a539 | 66 | ledr = 1; |
Arnoud113 | 6:1a410879a539 | 67 | ledg = 0; |
Arnoud113 | 6:1a410879a539 | 68 | } |
Arnoud113 | 0:1816743ba8ba | 69 | return referenceVelocity; |
Arnoud113 | 0:1816743ba8ba | 70 | } |
Arnoud113 | 0:1816743ba8ba | 71 | |
Arnoud113 | 6:1a410879a539 | 72 | float FeedForwardControl(float &referenceVelocity) |
Arnoud113 | 0:1816743ba8ba | 73 | { |
Arnoud113 | 0:1816743ba8ba | 74 | // very simple linear feed-forward control |
Arnoud113 | 0:1816743ba8ba | 75 | const float MotorGain=8.4; // unit: (rad/s) / PWM |
Arnoud113 | 6:1a410879a539 | 76 | float motorValue = referenceVelocity / MotorGain; |
Arnoud113 | 5:a3848a66a4df | 77 | |
Arnoud113 | 5:a3848a66a4df | 78 | // float referenceVelocity = GetReferenceVelocity(); |
Arnoud113 | 6:1a410879a539 | 79 | |
Arnoud113 | 6:1a410879a539 | 80 | /* if(counts < referencePosition) |
Arnoud113 | 5:a3848a66a4df | 81 | { |
Arnoud113 | 5:a3848a66a4df | 82 | //float motorValue = FeedForwardControl(referenceVelocity); |
Arnoud113 | 5:a3848a66a4df | 83 | //SetMotor1(motorValue); |
Arnoud113 | 5:a3848a66a4df | 84 | motorValue = referenceVelocity / MotorGain; |
Arnoud113 | 5:a3848a66a4df | 85 | } |
Arnoud113 | 5:a3848a66a4df | 86 | else if (counts > referencePosition) |
Arnoud113 | 5:a3848a66a4df | 87 | { |
Arnoud113 | 5:a3848a66a4df | 88 | //float motorValue = b * FeedForwardControl(referenceVelocity); |
Arnoud113 | 5:a3848a66a4df | 89 | //SetMotor1(motorValue); |
Arnoud113 | 5:a3848a66a4df | 90 | |
Arnoud113 | 5:a3848a66a4df | 91 | motorValue= -1 * (referenceVelocity / MotorGain); |
Arnoud113 | 5:a3848a66a4df | 92 | } |
Arnoud113 | 5:a3848a66a4df | 93 | else |
Arnoud113 | 5:a3848a66a4df | 94 | { |
Arnoud113 | 5:a3848a66a4df | 95 | motorValue = 0; |
Arnoud113 | 6:1a410879a539 | 96 | }*/ |
Arnoud113 | 5:a3848a66a4df | 97 | |
Arnoud113 | 0:1816743ba8ba | 98 | return motorValue; |
Arnoud113 | 0:1816743ba8ba | 99 | } |
Arnoud113 | 0:1816743ba8ba | 100 | |
Arnoud113 | 0:1816743ba8ba | 101 | void SetMotor1(float motorValue) |
Arnoud113 | 0:1816743ba8ba | 102 | { |
Arnoud113 | 0:1816743ba8ba | 103 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Arnoud113 | 0:1816743ba8ba | 104 | // bits for motor 1. Positive value makes motor rotating |
Arnoud113 | 0:1816743ba8ba | 105 | // clockwise. motorValues outside range are truncated to |
Arnoud113 | 0:1816743ba8ba | 106 | // within range |
Arnoud113 | 1:659489c32e75 | 107 | if (motorValue >=0) motor1DC= 1; |
Arnoud113 | 0:1816743ba8ba | 108 | else motor1DC=0; |
Arnoud113 | 0:1816743ba8ba | 109 | if (fabs(motorValue)>1) motor1PWM = 1; |
Arnoud113 | 0:1816743ba8ba | 110 | else motor1PWM = fabs(motorValue); |
Arnoud113 | 0:1816743ba8ba | 111 | } |
Arnoud113 | 0:1816743ba8ba | 112 | |
Arnoud113 | 1:659489c32e75 | 113 | void MeasureAndControl(void) |
Arnoud113 | 1:659489c32e75 | 114 | { |
Arnoud113 | 1:659489c32e75 | 115 | float referenceVelocity = GetReferenceVelocity(); |
Arnoud113 | 4:983b50758735 | 116 | int referencePosition = GetReferencePosition(); |
Arnoud113 | 5:a3848a66a4df | 117 | float motorValue = FeedForwardControl(referenceVelocity); |
Arnoud113 | 5:a3848a66a4df | 118 | SetMotor1(motorValue); |
Arnoud113 | 5:a3848a66a4df | 119 | |
Arnoud113 | 5:a3848a66a4df | 120 | /*int counts = Encoder.getPulses(); |
Arnoud113 | 4:983b50758735 | 121 | |
Arnoud113 | 4:983b50758735 | 122 | |
Arnoud113 | 4:983b50758735 | 123 | |
Arnoud113 | 4:983b50758735 | 124 | if(counts < referencePosition) |
Arnoud113 | 4:983b50758735 | 125 | { |
Arnoud113 | 4:983b50758735 | 126 | float motorValue = FeedForwardControl(referenceVelocity); |
Arnoud113 | 4:983b50758735 | 127 | SetMotor1(motorValue); |
Arnoud113 | 4:983b50758735 | 128 | } |
Arnoud113 | 4:983b50758735 | 129 | else |
Arnoud113 | 4:983b50758735 | 130 | { |
Arnoud113 | 4:983b50758735 | 131 | int b = -1; |
Arnoud113 | 4:983b50758735 | 132 | float motorValue = b * FeedForwardControl(referenceVelocity); |
Arnoud113 | 4:983b50758735 | 133 | SetMotor1(motorValue); |
Arnoud113 | 5:a3848a66a4df | 134 | } */ |
Arnoud113 | 4:983b50758735 | 135 | |
Arnoud113 | 1:659489c32e75 | 136 | } |
Arnoud113 | 1:659489c32e75 | 137 | |
Arnoud113 | 3:cc3766838777 | 138 | |
Arnoud113 | 3:cc3766838777 | 139 | |
Arnoud113 | 0:1816743ba8ba | 140 | int main() |
Arnoud113 | 0:1816743ba8ba | 141 | { |
Arnoud113 | 0:1816743ba8ba | 142 | pc.baud(115200); |
Arnoud113 | 2:abd40da4a5e2 | 143 | QEI Encoder(D12,D13,NC,32); |
Arnoud113 | 3:cc3766838777 | 144 | |
Arnoud113 | 3:cc3766838777 | 145 | ledr = 1; |
Arnoud113 | 3:cc3766838777 | 146 | ledb = 1; |
Arnoud113 | 3:cc3766838777 | 147 | ledg = 1; |
Arnoud113 | 0:1816743ba8ba | 148 | |
Arnoud113 | 6:1a410879a539 | 149 | |
Arnoud113 | 6:1a410879a539 | 150 | |
Arnoud113 | 2:abd40da4a5e2 | 151 | controller.attach(&MeasureAndControl, 0.1); |
Arnoud113 | 3:cc3766838777 | 152 | |
Arnoud113 | 1:659489c32e75 | 153 | while(1) |
Arnoud113 | 3:cc3766838777 | 154 | { |
Arnoud113 | 2:abd40da4a5e2 | 155 | int counts = Encoder.getPulses(); |
Arnoud113 | 2:abd40da4a5e2 | 156 | float rV = GetReferenceVelocity(); |
Arnoud113 | 2:abd40da4a5e2 | 157 | float mV = FeedForwardControl(rV); |
Arnoud113 | 4:983b50758735 | 158 | int rP = GetReferencePosition(); |
Arnoud113 | 2:abd40da4a5e2 | 159 | |
Arnoud113 | 5:a3848a66a4df | 160 | pc.printf("\r reference velocity: %f. Reference Position: %i Motor Value is: %f number of counts: %i\n",rV,rP,mV,counts); |
Arnoud113 | 2:abd40da4a5e2 | 161 | } |
Arnoud113 | 2:abd40da4a5e2 | 162 | |
Arnoud113 | 0:1816743ba8ba | 163 | } |
Arnoud113 | 0:1816743ba8ba | 164 | |
Arnoud113 | 0:1816743ba8ba | 165 |