David Pasztor / Mbed 2 deprecated Motor_control

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:   mbed-rtos mbed

main.cpp@45:bfd7cbd41957, 2017-03-19 (annotated)

Committer:
NKarandey
Date:
Sun Mar 19 23:46:22 2017 +0000
Revision:
45:bfd7cbd41957
Parent:
44:0b92f72641d7
Child:
46:5c50778bb2d5
Experiments with velocity controller; ; The previous main file is main1.cpp; ; The controller works on some speeds, but not others.; Have an issue with total error growing too quickly on large speeds.; Also need to try adding differential term.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NKarandey 43:8b6b4040e635 1 #include <cmath>
NKarandey 43:8b6b4040e635 2
NKarandey 41:d52445129908 3 #include "mbed.h"
NKarandey 41:d52445129908 4 #include "rtos.h"
NKarandey 43:8b6b4040e635 5
NKarandey 41:d52445129908 6 #include "definitions.h"
NKarandey 41:d52445129908 7 #include "motorControl.h"
NKarandey 41:d52445129908 8 #include "parser.h"
NKarandey 43:8b6b4040e635 9
NKarandey 45:bfd7cbd41957 10 //#define print(...) sprintf((char*)userOutput, __VA_ARGS__); outputRequested = true
NKarandey 45:bfd7cbd41957 11
NKarandey 43:8b6b4040e635 12 Mutex mutex;
NKarandey 45:bfd7cbd41957 13 Thread motorControlThread;
NKarandey 45:bfd7cbd41957 14
NKarandey 45:bfd7cbd41957 15 Ticker monitorTicker;
NKarandey 45:bfd7cbd41957 16 void monitor();
NKarandey 41:d52445129908 17
NKarandey 41:d52445129908 18 volatile float w3 = 0; //Angular velocity
NKarandey 45:bfd7cbd41957 19 volatile float duty = 0.2;
NKarandey 41:d52445129908 20 volatile int count_i3 = 0;
NKarandey 41:d52445129908 21
NKarandey 43:8b6b4040e635 22 volatile char userInput[256];
NKarandey 43:8b6b4040e635 23 volatile bool commandReady = false;
NKarandey 43:8b6b4040e635 24 ParseResult parseResult;
NKarandey 43:8b6b4040e635 25 volatile bool readyForCommand = true;
NKarandey 43:8b6b4040e635 26
NKarandey 43:8b6b4040e635 27 void rotateWith(float r, float v) {}
NKarandey 44:0b92f72641d7 28
NKarandey 43:8b6b4040e635 29 void playTunes(const vector<float>& tunes) {}
NKarandey 43:8b6b4040e635 30
NKarandey 45:bfd7cbd41957 31 volatile char userOutput[256];
NKarandey 45:bfd7cbd41957 32 volatile bool outputRequested=false;
NKarandey 45:bfd7cbd41957 33
NKarandey 45:bfd7cbd41957 34 //void serialOut() {
NKarandey 45:bfd7cbd41957 35 // while (true) {
NKarandey 45:bfd7cbd41957 36 // if (outputRequested) {
NKarandey 45:bfd7cbd41957 37 //// mutex.lock();
NKarandey 45:bfd7cbd41957 38 // printf("%s\n\r", userOutput);
NKarandey 45:bfd7cbd41957 39 // outputRequested = false;
NKarandey 45:bfd7cbd41957 40 //// mutex.unlock();
NKarandey 45:bfd7cbd41957 41 // }
NKarandey 45:bfd7cbd41957 42 // }
NKarandey 45:bfd7cbd41957 43 //}
NKarandey 45:bfd7cbd41957 44 //void runMotor() {
NKarandey 45:bfd7cbd41957 45 // while (true) {
NKarandey 45:bfd7cbd41957 46 // motorOut((state-orState+lead+6)%6, duty);
NKarandey 45:bfd7cbd41957 47 // Thread::wait(5);
NKarandey 45:bfd7cbd41957 48 // }
NKarandey 45:bfd7cbd41957 49 // print("On");
NKarandey 45:bfd7cbd41957 50 //}
NKarandey 45:bfd7cbd41957 51
NKarandey 45:bfd7cbd41957 52 //void motorSwitchOn() {
NKarandey 45:bfd7cbd41957 53 // motorControlThread.start(callback(runMotor));
NKarandey 45:bfd7cbd41957 54 //}
NKarandey 45:bfd7cbd41957 55 //
NKarandey 45:bfd7cbd41957 56 //void motorSwitchOff() {
NKarandey 45:bfd7cbd41957 57 // motorControlThread.terminate();
NKarandey 45:bfd7cbd41957 58 //}
NKarandey 45:bfd7cbd41957 59
NKarandey 43:8b6b4040e635 60
NKarandey 41:d52445129908 61 volatile int CHA_state = 0x00;
NKarandey 41:d52445129908 62 volatile int CHB_state = 0x00;
NKarandey 41:d52445129908 63 volatile int CH_state = 0x00;
NKarandey 41:d52445129908 64 volatile int CH_state_prev = 0x00;
NKarandey 41:d52445129908 65
davidanderle 42:2ee563cd6223 66 volatile float diskPosition = 0.0; //in degrees
NKarandey 41:d52445129908 67
NKarandey 41:d52445129908 68 Timer dt_I3;
NKarandey 41:d52445129908 69 Ticker controlTicker;
NKarandey 44:0b92f72641d7 70 Ticker motorOutTicker;
NKarandey 45:bfd7cbd41957 71 Ticker lifeTicker;
NKarandey 45:bfd7cbd41957 72
davidanderle 42:2ee563cd6223 73 volatile float currentRevs = 0.0; //number of revs done
davidanderle 42:2ee563cd6223 74 volatile float goalRevs = 10.0; //number of revs to do
davidanderle 42:2ee563cd6223 75 volatile float prevError = 0.0; //previous error in position
davidanderle 42:2ee563cd6223 76 volatile double dError = 0.0;
davidanderle 42:2ee563cd6223 77 volatile float currentError = 0.0; //current error in position
NKarandey 41:d52445129908 78
NKarandey 41:d52445129908 79 #define kp 0.012f
davidanderle 42:2ee563cd6223 80 #define kd 0.02f
NKarandey 41:d52445129908 81 #define k 10.0f
NKarandey 44:0b92f72641d7 82
NKarandey 45:bfd7cbd41957 83
NKarandey 45:bfd7cbd41957 84
NKarandey 44:0b92f72641d7 85 //given in ms,used to call the PD controller
NKarandey 45:bfd7cbd41957 86 const float dtControl = 0.2f;
NKarandey 44:0b92f72641d7 87 // Period of giving power to the motor
NKarandey 45:bfd7cbd41957 88 const float dtMotor = 0.05f;
NKarandey 41:d52445129908 89
NKarandey 44:0b92f72641d7 90 volatile bool commandFinished = false;
NKarandey 44:0b92f72641d7 91
NKarandey 45:bfd7cbd41957 92 float min(float a, float b) {
NKarandey 45:bfd7cbd41957 93 return a<b ? a : b;
NKarandey 45:bfd7cbd41957 94 }
NKarandey 45:bfd7cbd41957 95
NKarandey 45:bfd7cbd41957 96 float computeDelta(float p) {
NKarandey 45:bfd7cbd41957 97 float dMin = 0.1;
NKarandey 45:bfd7cbd41957 98 float dMax = 0.4;
NKarandey 45:bfd7cbd41957 99
NKarandey 45:bfd7cbd41957 100 if (p > dMax)
NKarandey 45:bfd7cbd41957 101 return dMax;
NKarandey 45:bfd7cbd41957 102 else if (p > dMin)
NKarandey 45:bfd7cbd41957 103 return p;
NKarandey 45:bfd7cbd41957 104 else
NKarandey 45:bfd7cbd41957 105 return dMin;
NKarandey 44:0b92f72641d7 106 }
NKarandey 44:0b92f72641d7 107
NKarandey 45:bfd7cbd41957 108 const float VKp = 1.1f;
NKarandey 45:bfd7cbd41957 109 const float VKi = 0.01f;
NKarandey 45:bfd7cbd41957 110 const float VKd = -0.000f;
NKarandey 45:bfd7cbd41957 111 volatile float vPrevError = 0.0f;
NKarandey 45:bfd7cbd41957 112 volatile float targetV = 300.0f;
NKarandey 45:bfd7cbd41957 113 volatile float dutyParam = 0.0f;
NKarandey 45:bfd7cbd41957 114 volatile float totalVErr = 0.0f;
NKarandey 45:bfd7cbd41957 115 volatile float vPrevErr = 0.0f;
NKarandey 45:bfd7cbd41957 116 volatile float vErr = 0;
NKarandey 44:0b92f72641d7 117 void controlVelocity() {
NKarandey 45:bfd7cbd41957 118 vErr = (targetV - w3) / targetV;
NKarandey 45:bfd7cbd41957 119 float dErr = vErr - vPrevErr;
NKarandey 45:bfd7cbd41957 120 totalVErr += vErr;
NKarandey 45:bfd7cbd41957 121 dutyParam = VKp*vErr + VKi*totalVErr + VKd*dErr;
NKarandey 45:bfd7cbd41957 122 duty = computeDelta(dutyParam);
NKarandey 45:bfd7cbd41957 123 if (vErr < 0) {
NKarandey 45:bfd7cbd41957 124 // dutyParam = 0.1f;
NKarandey 45:bfd7cbd41957 125 // lead = -lead;
NKarandey 45:bfd7cbd41957 126 }
NKarandey 45:bfd7cbd41957 127 vPrevErr = vErr;
NKarandey 45:bfd7cbd41957 128 // dutyParam = 2.0f * VKp * vErr;
NKarandey 45:bfd7cbd41957 129 // if (dutyParam > 0.0f) {
NKarandey 45:bfd7cbd41957 130 // duty = min(1.0f, dutyParam / maxDutyParam);
NKarandey 45:bfd7cbd41957 131 // } else {
NKarandey 45:bfd7cbd41957 132 // printf("Reversed the motor!\n\r");
NKarandey 45:bfd7cbd41957 133 // duty = min(1.0f, -dutyParam / maxDutyParam);
NKarandey 45:bfd7cbd41957 134 // lead *= -1;
NKarandey 45:bfd7cbd41957 135 // }
NKarandey 45:bfd7cbd41957 136 // duty = 0.2f;
NKarandey 44:0b92f72641d7 137 }
NKarandey 44:0b92f72641d7 138
NKarandey 45:bfd7cbd41957 139 void stopCommand() {
NKarandey 45:bfd7cbd41957 140 commandFinished = true;
NKarandey 45:bfd7cbd41957 141 lifeTicker.detach();
NKarandey 45:bfd7cbd41957 142 controlTicker.detach();
NKarandey 45:bfd7cbd41957 143 }
NKarandey 45:bfd7cbd41957 144
NKarandey 45:bfd7cbd41957 145 void setVelocity(float v) {
NKarandey 45:bfd7cbd41957 146 targetV = v;
NKarandey 45:bfd7cbd41957 147 goalRevs = 900.0f;
NKarandey 45:bfd7cbd41957 148 printf("Spinning with V=%.2f\n\r", targetV);
NKarandey 45:bfd7cbd41957 149 state = updateState();
NKarandey 45:bfd7cbd41957 150 motorOut((state-orState+lead+6)%6, 0.3f);
NKarandey 45:bfd7cbd41957 151 controlTicker.attach(&controlVelocity, dtControl);
NKarandey 45:bfd7cbd41957 152 // motorSwitchOn();
NKarandey 45:bfd7cbd41957 153 // motorOutTicker.attach(&runMotor, dtMotor);
NKarandey 45:bfd7cbd41957 154 lifeTicker.attach(&stopCommand, 10);
NKarandey 41:d52445129908 155 }
NKarandey 41:d52445129908 156
NKarandey 45:bfd7cbd41957 157 void serialThread() {
NKarandey 45:bfd7cbd41957 158 while(true) {
NKarandey 45:bfd7cbd41957 159 // if (readyForCommand) {
NKarandey 45:bfd7cbd41957 160 // scanf("%s", userInput);
NKarandey 45:bfd7cbd41957 161 // ParseResult curr = parse((char *) userInput);
NKarandey 45:bfd7cbd41957 162 // if (curr.success) {
NKarandey 45:bfd7cbd41957 163 // mutex.lock();
NKarandey 45:bfd7cbd41957 164 // commandReady = true;
NKarandey 45:bfd7cbd41957 165 // parseResult = curr;
NKarandey 45:bfd7cbd41957 166 // mutex.unlock();
NKarandey 45:bfd7cbd41957 167 // }
NKarandey 45:bfd7cbd41957 168 // }
NKarandey 45:bfd7cbd41957 169 // Thread::wait(10);
NKarandey 45:bfd7cbd41957 170 // if (outputRequested) {
NKarandey 45:bfd7cbd41957 171 printf("V = %.2f; delta = %.2f dutyParam = %.2f vErr = %.2f vTotalErr= %.2f\n\r", w3, duty, dutyParam, vErr, totalVErr);
NKarandey 45:bfd7cbd41957 172 // printf("Current pos: %.2f\n\r", diskPosition);
NKarandey 45:bfd7cbd41957 173 Thread::wait(2000);
NKarandey 45:bfd7cbd41957 174 // printf("%s\n\r", userOutput);
NKarandey 45:bfd7cbd41957 175 // outputRequested = false;
NKarandey 45:bfd7cbd41957 176 // }
NKarandey 45:bfd7cbd41957 177 }
NKarandey 44:0b92f72641d7 178 }
NKarandey 41:d52445129908 179 void i1rise(){
NKarandey 41:d52445129908 180 state = updateState();
NKarandey 45:bfd7cbd41957 181 // motorOut((state-orState+lead+6)%6, duty);
NKarandey 41:d52445129908 182
NKarandey 45:bfd7cbd41957 183 if (I3.read() == 1) //Only count revolutions if the
NKarandey 45:bfd7cbd41957 184 count_i3++; // rotor spins forward
NKarandey 41:d52445129908 185 }
davidanderle 42:2ee563cd6223 186 //TODO merge with i_edge by measuring angular velocity in i1rise.
NKarandey 41:d52445129908 187 void i3rise(){
NKarandey 41:d52445129908 188 state = updateState();
NKarandey 45:bfd7cbd41957 189 // motorOut((state-orState+lead+6)%6, duty);
NKarandey 45:bfd7cbd41957 190 w3 = angle/dt_I3.read(); //Calc angular velocity
NKarandey 41:d52445129908 191 dt_I3.reset();
NKarandey 41:d52445129908 192 }
NKarandey 41:d52445129908 193
davidanderle 42:2ee563cd6223 194 void i_edge(){ //Upon status led interrupt, update
davidanderle 42:2ee563cd6223 195 state = updateState(); // the motor output
NKarandey 44:0b92f72641d7 196 // motorOut((state-orState+lead+6)%6, duty);
NKarandey 41:d52445129908 197 }
davidanderle 42:2ee563cd6223 198 //Todo: add comments on this fucntion
NKarandey 41:d52445129908 199 void updateDiskPosition() {
NKarandey 41:d52445129908 200 if (CH_state != CH_state_prev) {
NKarandey 45:bfd7cbd41957 201 float diff = CH_state - CH_state_prev;
NKarandey 45:bfd7cbd41957 202
NKarandey 41:d52445129908 203 CH_state_prev = CH_state;
NKarandey 45:bfd7cbd41957 204 // diskPosition += -diff * angularResolution;
NKarandey 41:d52445129908 205 if (abs(diff) == 1 || abs(diff) == 3) {
NKarandey 41:d52445129908 206 if (diff < 0)
NKarandey 41:d52445129908 207 diskPosition += angularResolution;
NKarandey 41:d52445129908 208 else
NKarandey 41:d52445129908 209 diskPosition -= angularResolution;
NKarandey 41:d52445129908 210 }
NKarandey 41:d52445129908 211 else if (abs(diff) == 2) {
NKarandey 41:d52445129908 212 if (diff < 0)
NKarandey 41:d52445129908 213 diskPosition += 2.0f * angularResolution;
NKarandey 41:d52445129908 214 else
NKarandey 41:d52445129908 215 diskPosition -= 2.0f * angularResolution;
NKarandey 41:d52445129908 216 }
NKarandey 41:d52445129908 217
NKarandey 41:d52445129908 218 if (diskPosition >= 360.0f) {
NKarandey 41:d52445129908 219 diskPosition -= 360.0f;
NKarandey 41:d52445129908 220 } else if (diskPosition < -360.0f) {
NKarandey 41:d52445129908 221 diskPosition += 360.0f;
NKarandey 41:d52445129908 222 }
NKarandey 41:d52445129908 223 }
NKarandey 41:d52445129908 224 }
NKarandey 41:d52445129908 225
NKarandey 41:d52445129908 226 void updateRelativeState() {
NKarandey 41:d52445129908 227 CH_state = relativeStateMap[CHB_state + 2*CHA_state];
NKarandey 41:d52445129908 228 }
NKarandey 41:d52445129908 229
NKarandey 41:d52445129908 230 void CHA_rise() {
NKarandey 41:d52445129908 231 CHA_state = 1;
NKarandey 41:d52445129908 232 updateRelativeState();
NKarandey 41:d52445129908 233 updateDiskPosition();
NKarandey 41:d52445129908 234 }
NKarandey 41:d52445129908 235 void CHA_fall() {
NKarandey 41:d52445129908 236 CHA_state = 0;
NKarandey 41:d52445129908 237 updateRelativeState();
NKarandey 41:d52445129908 238 updateDiskPosition();
NKarandey 41:d52445129908 239 }
NKarandey 41:d52445129908 240 void CHB_rise() {
NKarandey 41:d52445129908 241 CHB_state = 1;
NKarandey 41:d52445129908 242 updateRelativeState();
NKarandey 41:d52445129908 243 updateDiskPosition();
NKarandey 41:d52445129908 244 }
NKarandey 41:d52445129908 245 void CHB_fall() {
NKarandey 41:d52445129908 246 CHB_state = 0;
NKarandey 41:d52445129908 247 updateRelativeState();
NKarandey 41:d52445129908 248 updateDiskPosition();
NKarandey 41:d52445129908 249 }
NKarandey 41:d52445129908 250
NKarandey 45:bfd7cbd41957 251 int main() {
NKarandey 45:bfd7cbd41957 252 Thread serialHandler;
NKarandey 43:8b6b4040e635 253
NKarandey 45:bfd7cbd41957 254 // motorControlThread.start(callback(runMotor));
NKarandey 41:d52445129908 255 motorHome(); //Initialise motor before any interrupt
NKarandey 45:bfd7cbd41957 256 // stopMotor();
NKarandey 41:d52445129908 257 dt_I3.start(); //Start the time counters for velocity
NKarandey 41:d52445129908 258
NKarandey 41:d52445129908 259
NKarandey 41:d52445129908 260 I1.rise(&i1rise); //Assign interrupt handlers for LEDs
NKarandey 41:d52445129908 261 I1.fall(&i_edge);
NKarandey 41:d52445129908 262 I2.rise(&i_edge);
NKarandey 41:d52445129908 263 I2.fall(&i_edge);
NKarandey 41:d52445129908 264 I3.rise(&i3rise);
NKarandey 41:d52445129908 265 I3.fall(&i_edge);
NKarandey 41:d52445129908 266
davidanderle 42:2ee563cd6223 267 CHA.rise(&CHA_rise); //Assign interrupt handlers for
davidanderle 42:2ee563cd6223 268 CHA.fall(&CHA_fall); // precision angle LEDs
NKarandey 41:d52445129908 269 CHB.rise(&CHB_rise);
NKarandey 41:d52445129908 270 CHB.fall(&CHB_fall);
NKarandey 41:d52445129908 271
NKarandey 45:bfd7cbd41957 272 // printf("Ready\n\r");
NKarandey 45:bfd7cbd41957 273 serialHandler.start(callback(serialThread));
NKarandey 45:bfd7cbd41957 274 controlTicker.attach(&controlVelocity, dtControl);
NKarandey 45:bfd7cbd41957 275 // setVelocity(100);
NKarandey 44:0b92f72641d7 276 while (true) {
NKarandey 45:bfd7cbd41957 277 motorOut((state-orState+lead+6)%6, duty);
NKarandey 45:bfd7cbd41957 278 // Values of 1 or 10 seem to make no difference for v = 100
NKarandey 45:bfd7cbd41957 279 Thread::wait(10);
NKarandey 45:bfd7cbd41957 280 // printf("Current pos: %.2f\n\r", diskPosition);
NKarandey 41:d52445129908 281 }
NKarandey 41:d52445129908 282 return 0;
NKarandey 43:8b6b4040e635 283 }
NKarandey 43:8b6b4040e635 284
NKarandey 43:8b6b4040e635 285
NKarandey 45:bfd7cbd41957 286 //
NKarandey 45:bfd7cbd41957 287 //void monitor() {
NKarandey 45:bfd7cbd41957 288 //// print("Current speed %.2f\n\r", w3);
NKarandey 45:bfd7cbd41957 289 // print("Current revs: %.2f\n\r", currentRevs);
NKarandey 45:bfd7cbd41957 290 //}