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@47:49e6c3fb25dc, 2017-03-24 (annotated)

Committer:
davidanderle
Date:
Fri Mar 24 14:48:27 2017 +0000
Revision:
47:49e6c3fb25dc
Parent:
46:5c50778bb2d5
Child:
48:02d5565b8fac
playing with velocity control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NKarandey 43:8b6b4040e635 1 #include <cmath>
NKarandey 46:5c50778bb2d5 2 #include <vector>
NKarandey 46:5c50778bb2d5 3 //#include "mbed.h"
NKarandey 46:5c50778bb2d5 4 //#include "rtos.h"
NKarandey 43:8b6b4040e635 5
NKarandey 41:d52445129908 6 #include "definitions.h"
NKarandey 41:d52445129908 7 #include "motorControl.h"
davidanderle 47:49e6c3fb25dc 8 //#include "parser.h"
davidanderle 47:49e6c3fb25dc 9 //#include "serialHandler.h"
NKarandey 45:bfd7cbd41957 10
davidanderle 47:49e6c3fb25dc 11 //Mutex mutex;
davidanderle 47:49e6c3fb25dc 12 //Thread serialOutputer;
davidanderle 47:49e6c3fb25dc 13 //
davidanderle 47:49e6c3fb25dc 14 //Thread serialInputer;
NKarandey 41:d52445129908 15
NKarandey 46:5c50778bb2d5 16 #define print(...) sprintf((char*)userOutput, __VA_ARGS__); outputRequested = true;
NKarandey 43:8b6b4040e635 17
davidanderle 47:49e6c3fb25dc 18 //ParseResult parseResult;
davidanderle 47:49e6c3fb25dc 19 //volatile char userInput[256];
davidanderle 47:49e6c3fb25dc 20 //volatile char userOutput[256];
davidanderle 47:49e6c3fb25dc 21 //volatile bool outputRequested=false;
davidanderle 47:49e6c3fb25dc 22 //
davidanderle 47:49e6c3fb25dc 23 //volatile bool commandReady = false;
davidanderle 47:49e6c3fb25dc 24 //volatile bool readyForCommand = true;
davidanderle 47:49e6c3fb25dc 25 //void serialOut() {
davidanderle 47:49e6c3fb25dc 26 // while(true) {
davidanderle 47:49e6c3fb25dc 27 // if (outputRequested) {
davidanderle 47:49e6c3fb25dc 28 // printf("%s\n\r", userOutput);
davidanderle 47:49e6c3fb25dc 29 // mutex.lock();
davidanderle 47:49e6c3fb25dc 30 // outputRequested = false;
davidanderle 47:49e6c3fb25dc 31 // mutex.unlock();
davidanderle 47:49e6c3fb25dc 32 // }
davidanderle 47:49e6c3fb25dc 33 // }
davidanderle 47:49e6c3fb25dc 34 //}
NKarandey 45:bfd7cbd41957 35
davidanderle 47:49e6c3fb25dc 36 //void serialIn() {
davidanderle 47:49e6c3fb25dc 37 // while(true) {
davidanderle 47:49e6c3fb25dc 38 // if (readyForCommand) {
davidanderle 47:49e6c3fb25dc 39 // scanf("%s", userInput);
davidanderle 47:49e6c3fb25dc 40 // ParseResult curr = parse((char *) userInput);
davidanderle 47:49e6c3fb25dc 41 // if (curr.success) {
davidanderle 47:49e6c3fb25dc 42 // mutex.lock();
davidanderle 47:49e6c3fb25dc 43 // commandReady = true;
davidanderle 47:49e6c3fb25dc 44 // parseResult = curr;
davidanderle 47:49e6c3fb25dc 45 // mutex.unlock();
davidanderle 47:49e6c3fb25dc 46 // printf("Got command\n\r");
davidanderle 47:49e6c3fb25dc 47 // }
davidanderle 47:49e6c3fb25dc 48 // }
davidanderle 47:49e6c3fb25dc 49 // }
davidanderle 47:49e6c3fb25dc 50 //}
NKarandey 45:bfd7cbd41957 51
NKarandey 46:5c50778bb2d5 52 volatile float w3 = 0; //Angular velocity
NKarandey 46:5c50778bb2d5 53 volatile int count_i3 = 0;
NKarandey 46:5c50778bb2d5 54
NKarandey 41:d52445129908 55 volatile int CHA_state = 0x00;
NKarandey 41:d52445129908 56 volatile int CHB_state = 0x00;
NKarandey 41:d52445129908 57 volatile int CH_state = 0x00;
NKarandey 41:d52445129908 58 volatile int CH_state_prev = 0x00;
NKarandey 46:5c50778bb2d5 59
NKarandey 46:5c50778bb2d5 60 volatile float diskPosition = 0.0; //in degrees
NKarandey 46:5c50778bb2d5 61 volatile float velocityDuty = 0.0;
NKarandey 46:5c50778bb2d5 62
NKarandey 41:d52445129908 63 Timer dt_I3;
NKarandey 41:d52445129908 64 Ticker controlTicker;
davidanderle 47:49e6c3fb25dc 65 //Ticker pwmPeriod;
davidanderle 47:49e6c3fb25dc 66 //Ticker pwmTon;
davidanderle 47:49e6c3fb25dc 67 //Ticker motorTicker;
davidanderle 47:49e6c3fb25dc 68 //float dtMotor = 0.1;
NKarandey 46:5c50778bb2d5 69
davidanderle 47:49e6c3fb25dc 70 //volatile float currentRevs = 0.0; //number of revs done
davidanderle 47:49e6c3fb25dc 71 //volatile float goalRevs = 130.5;
davidanderle 47:49e6c3fb25dc 72 //volatile float prevError = 0.0;
davidanderle 47:49e6c3fb25dc 73 //volatile double dError = 0.0;
davidanderle 47:49e6c3fb25dc 74 //volatile float currentError = 0.0;
davidanderle 47:49e6c3fb25dc 75 //volatile int phaseLead = 120;
NKarandey 46:5c50778bb2d5 76
NKarandey 46:5c50778bb2d5 77 //Make variables constant?
davidanderle 47:49e6c3fb25dc 78 //#define kp 0.012f
davidanderle 47:49e6c3fb25dc 79 //#define kd 0.019f //0.5f, 0.02
davidanderle 47:49e6c3fb25dc 80 //#define k 10.0f
davidanderle 47:49e6c3fb25dc 81 //#define dt 0.002f //given in ms, used to call the PID c.
davidanderle 47:49e6c3fb25dc 82 //volatile float velocityPeriod = 0.004; //0.4ms (velocityPwm) >> 40us (motorPwm)
davidanderle 47:49e6c3fb25dc 83 //volatile float velocityTon = 0.0;
davidanderle 47:49e6c3fb25dc 84 //volatile bool pwmOut = 1;
NKarandey 46:5c50778bb2d5 85
davidanderle 47:49e6c3fb25dc 86 //inline void velocityPwmTon(){
davidanderle 47:49e6c3fb25dc 87 // pwmOut = 0;
davidanderle 47:49e6c3fb25dc 88 // pwmTon.detach();
davidanderle 47:49e6c3fb25dc 89 //}
davidanderle 47:49e6c3fb25dc 90 //
davidanderle 47:49e6c3fb25dc 91 //inline void velocityPwmPeriod(){
davidanderle 47:49e6c3fb25dc 92 // pwmOut = 1;
davidanderle 47:49e6c3fb25dc 93 // pwmTon.attach(&velocityPwmTon, velocityTon);
davidanderle 47:49e6c3fb25dc 94 //// debugCounter++;
davidanderle 47:49e6c3fb25dc 95 //}
davidanderle 47:49e6c3fb25dc 96 //
davidanderle 47:49e6c3fb25dc 97 //void startVelocityPwm(float velocityPeriod){
davidanderle 47:49e6c3fb25dc 98 // pwmPeriod.attach(&velocityPwmPeriod, velocityPeriod);
davidanderle 47:49e6c3fb25dc 99 //}
NKarandey 46:5c50778bb2d5 100
davidanderle 47:49e6c3fb25dc 101 //void control(){
davidanderle 47:49e6c3fb25dc 102 // if (w3 > 300) {
davidanderle 47:49e6c3fb25dc 103 // lead = 2;
davidanderle 47:49e6c3fb25dc 104 // return;
davidanderle 47:49e6c3fb25dc 105 // }
davidanderle 47:49e6c3fb25dc 106 // prevError = currentError;
davidanderle 47:49e6c3fb25dc 107 // currentRevs = diskPosition / 360 + count_i3; // 1/360degr + 2pi*revs
davidanderle 47:49e6c3fb25dc 108 // currentError = goalRevs - currentRevs;
davidanderle 47:49e6c3fb25dc 109 // dError = (currentError - prevError)/dt;
davidanderle 47:49e6c3fb25dc 110 // velocityDuty = k*(kp*currentError + kd*dError);
davidanderle 47:49e6c3fb25dc 111 // if (velocityDuty > 0) lead = -2;
davidanderle 47:49e6c3fb25dc 112 // else {
davidanderle 47:49e6c3fb25dc 113 // lead = 2;
davidanderle 47:49e6c3fb25dc 114 // velocityDuty = -velocityDuty;
davidanderle 47:49e6c3fb25dc 115 // }
davidanderle 47:49e6c3fb25dc 116 // if(velocityDuty > 1)
davidanderle 47:49e6c3fb25dc 117 // velocityPeriod = 1;
davidanderle 47:49e6c3fb25dc 118 //
davidanderle 47:49e6c3fb25dc 119 // velocityTon = velocityPeriod * velocityDuty;
davidanderle 47:49e6c3fb25dc 120 //}
davidanderle 47:49e6c3fb25dc 121
davidanderle 47:49e6c3fb25dc 122 const float VKp = 0.9f; //0.054, 1.1, 0.012
davidanderle 47:49e6c3fb25dc 123 const float VKi = 0.001f; //0.0054, 0.01
davidanderle 47:49e6c3fb25dc 124 const float VKd = 0.05f; //0.0135, 0.02
davidanderle 47:49e6c3fb25dc 125
davidanderle 47:49e6c3fb25dc 126 volatile float targetV = 300.0f;
davidanderle 47:49e6c3fb25dc 127 volatile float totalVErr = 0.0f;
davidanderle 47:49e6c3fb25dc 128 volatile float vPrevErr = 0.0f;
davidanderle 47:49e6c3fb25dc 129 volatile float vErr = 0.0f;
davidanderle 47:49e6c3fb25dc 130 volatile float dErr = 0.0f;
davidanderle 47:49e6c3fb25dc 131
davidanderle 47:49e6c3fb25dc 132 const float dtControl = 0.01f;
davidanderle 47:49e6c3fb25dc 133 //Thread::wait(0.1) in the main for calling motorOut
davidanderle 47:49e6c3fb25dc 134
davidanderle 47:49e6c3fb25dc 135 void controlVelocity(){
davidanderle 47:49e6c3fb25dc 136 vErr = targetV - w3;
davidanderle 47:49e6c3fb25dc 137 dErr = (vErr - vPrevErr)/dtControl;
davidanderle 47:49e6c3fb25dc 138 totalVErr += vErr*dtControl;
davidanderle 47:49e6c3fb25dc 139 velocityDuty = VKp*vErr + VKi*totalVErr + VKd*dErr;
davidanderle 47:49e6c3fb25dc 140 if(velocityDuty < 0){
davidanderle 47:49e6c3fb25dc 141 velocityDuty = -velocityDuty;
NKarandey 46:5c50778bb2d5 142 lead = 2;
NKarandey 46:5c50778bb2d5 143 }
davidanderle 47:49e6c3fb25dc 144 else
davidanderle 47:49e6c3fb25dc 145 lead = -2;
davidanderle 47:49e6c3fb25dc 146 vPrevErr = vErr;
NKarandey 45:bfd7cbd41957 147 }
NKarandey 45:bfd7cbd41957 148
davidanderle 47:49e6c3fb25dc 149 //Timer profiler;
davidanderle 47:49e6c3fb25dc 150 //volatile float profilerDt = 0;
davidanderle 47:49e6c3fb25dc 151
davidanderle 47:49e6c3fb25dc 152 //takes 11-12us
NKarandey 46:5c50778bb2d5 153 inline void i3rise(){
davidanderle 47:49e6c3fb25dc 154 // profiler.reset();
davidanderle 47:49e6c3fb25dc 155 // profiler.start();
davidanderle 47:49e6c3fb25dc 156 state = updateState();
davidanderle 47:49e6c3fb25dc 157 motorOut((state-orState+lead+6)%6, velocityDuty);
NKarandey 46:5c50778bb2d5 158 w3 = angle/dt_I3.read(); //Calc angular velocity
NKarandey 46:5c50778bb2d5 159 dt_I3.reset();
NKarandey 46:5c50778bb2d5 160
NKarandey 46:5c50778bb2d5 161 if (I2.read() == 1) //Only count revolutions if the
NKarandey 46:5c50778bb2d5 162 count_i3++; // rotor spins forward
davidanderle 47:49e6c3fb25dc 163 // profiler.stop();
davidanderle 47:49e6c3fb25dc 164 // profilerDt = profiler.read_us();
davidanderle 47:49e6c3fb25dc 165 }
NKarandey 45:bfd7cbd41957 166
davidanderle 47:49e6c3fb25dc 167 void i_edge(){
davidanderle 47:49e6c3fb25dc 168 state = updateState();
davidanderle 47:49e6c3fb25dc 169 motorOut((state-orState+lead+6)%6, velocityDuty);
davidanderle 47:49e6c3fb25dc 170 }
NKarandey 46:5c50778bb2d5 171
NKarandey 46:5c50778bb2d5 172 void updateDiskPosition() {
NKarandey 46:5c50778bb2d5 173 if (CH_state != CH_state_prev) {
NKarandey 46:5c50778bb2d5 174 int diff = CH_state - CH_state_prev;
NKarandey 46:5c50778bb2d5 175
NKarandey 46:5c50778bb2d5 176 CH_state_prev = CH_state;
NKarandey 46:5c50778bb2d5 177 if (abs(diff) == 1 || abs(diff) == 3) {
NKarandey 46:5c50778bb2d5 178 if (diff < 0)
NKarandey 46:5c50778bb2d5 179 diskPosition += angularResolution;
NKarandey 46:5c50778bb2d5 180 else
NKarandey 46:5c50778bb2d5 181 diskPosition -= angularResolution;
NKarandey 46:5c50778bb2d5 182 }
NKarandey 46:5c50778bb2d5 183 else if (abs(diff) == 2) {
NKarandey 46:5c50778bb2d5 184 if (diff < 0)
NKarandey 46:5c50778bb2d5 185 diskPosition += 2.0f * angularResolution;
NKarandey 46:5c50778bb2d5 186 else
NKarandey 46:5c50778bb2d5 187 diskPosition -= 2.0f * angularResolution;
NKarandey 46:5c50778bb2d5 188 }
NKarandey 46:5c50778bb2d5 189
NKarandey 46:5c50778bb2d5 190 if (diskPosition >= 360.0f) {
NKarandey 46:5c50778bb2d5 191 diskPosition -= 360.0f;
NKarandey 46:5c50778bb2d5 192 } else if (diskPosition < -360.0f) {
NKarandey 46:5c50778bb2d5 193 diskPosition += 360.0f;
NKarandey 46:5c50778bb2d5 194 }
NKarandey 46:5c50778bb2d5 195 }
NKarandey 46:5c50778bb2d5 196 }
NKarandey 46:5c50778bb2d5 197
davidanderle 47:49e6c3fb25dc 198 void updateRelativeState() {
NKarandey 46:5c50778bb2d5 199 CH_state = relativeStateMap[CHB_state + 2*CHA_state];
NKarandey 44:0b92f72641d7 200 }
NKarandey 44:0b92f72641d7 201
NKarandey 46:5c50778bb2d5 202 inline void CHA_rise() {
NKarandey 46:5c50778bb2d5 203 CHA_state = 1;
NKarandey 46:5c50778bb2d5 204 updateRelativeState();
NKarandey 46:5c50778bb2d5 205 updateDiskPosition();
NKarandey 46:5c50778bb2d5 206 }
NKarandey 46:5c50778bb2d5 207 // Takes 5-6us
NKarandey 46:5c50778bb2d5 208 inline void CHA_fall() {
NKarandey 46:5c50778bb2d5 209 CHA_state = 0;
NKarandey 46:5c50778bb2d5 210 updateRelativeState();
NKarandey 46:5c50778bb2d5 211 updateDiskPosition();
NKarandey 46:5c50778bb2d5 212 }
NKarandey 46:5c50778bb2d5 213 inline void CHB_rise() {
NKarandey 46:5c50778bb2d5 214 CHB_state = 1;
NKarandey 46:5c50778bb2d5 215 updateRelativeState();
NKarandey 46:5c50778bb2d5 216 updateDiskPosition();
NKarandey 46:5c50778bb2d5 217 }
NKarandey 46:5c50778bb2d5 218 inline void CHB_fall() {
NKarandey 46:5c50778bb2d5 219 CHB_state = 0;
NKarandey 46:5c50778bb2d5 220 updateRelativeState();
NKarandey 46:5c50778bb2d5 221 updateDiskPosition();
NKarandey 46:5c50778bb2d5 222 }
NKarandey 46:5c50778bb2d5 223
davidanderle 47:49e6c3fb25dc 224 //inline void motorControl(){
davidanderle 47:49e6c3fb25dc 225 // if(pwmOut == 1) //Only control
davidanderle 47:49e6c3fb25dc 226 // precisionMotorOut(360-(((int)diskPosition+phaseLead)%360));
davidanderle 47:49e6c3fb25dc 227 //}
NKarandey 45:bfd7cbd41957 228
davidanderle 47:49e6c3fb25dc 229 //void playTune(float freq) {
davidanderle 47:49e6c3fb25dc 230 // motorPWMPeriod = 1.0f / freq;
davidanderle 47:49e6c3fb25dc 231 //// motorOut(0, 0.5);
davidanderle 47:49e6c3fb25dc 232 // Thread::wait(1000);
davidanderle 47:49e6c3fb25dc 233 //}
davidanderle 47:49e6c3fb25dc 234 //
davidanderle 47:49e6c3fb25dc 235 //void playTunes(const vector<float>& tunes) {
davidanderle 47:49e6c3fb25dc 236 // for (int i=0; i<tunes.size(); ++i) {
davidanderle 47:49e6c3fb25dc 237 // playTune(tunes[i]);
davidanderle 47:49e6c3fb25dc 238 // }
davidanderle 47:49e6c3fb25dc 239 //// motorPWMPeriod = defaultMotorPWMPeriod;
davidanderle 47:49e6c3fb25dc 240 // stopMotor(/*0*/);
davidanderle 47:49e6c3fb25dc 241 //}
NKarandey 41:d52445129908 242
NKarandey 45:bfd7cbd41957 243 int main() {
davidanderle 47:49e6c3fb25dc 244 I1.rise(&i_edge); //Assign interrupt handlers for LEDs
davidanderle 47:49e6c3fb25dc 245 I1.fall(&i_edge);
davidanderle 47:49e6c3fb25dc 246 I2.rise(&i_edge);
davidanderle 47:49e6c3fb25dc 247 I2.fall(&i_edge);
NKarandey 41:d52445129908 248 I3.rise(&i3rise);
davidanderle 47:49e6c3fb25dc 249 I3.fall(&i_edge);
NKarandey 46:5c50778bb2d5 250
davidanderle 47:49e6c3fb25dc 251 // LPins[0] = &L1L; //Define the pins for the pin array
davidanderle 47:49e6c3fb25dc 252 // LPins[1] = &L1H;
davidanderle 47:49e6c3fb25dc 253 // LPins[2] = &L2L;
davidanderle 47:49e6c3fb25dc 254 // LPins[3] = &L2H;
davidanderle 47:49e6c3fb25dc 255 // LPins[4] = &L3L;
davidanderle 47:49e6c3fb25dc 256 // LPins[5] = &L3H;
NKarandey 46:5c50778bb2d5 257
NKarandey 46:5c50778bb2d5 258 motorHome();
NKarandey 46:5c50778bb2d5 259
NKarandey 46:5c50778bb2d5 260 dt_I3.start(); //Start the time count for velocity
NKarandey 46:5c50778bb2d5 261 // controlTicker.attach(&control, dt);
davidanderle 47:49e6c3fb25dc 262 // motorTicker.attach(&motorControl, dtMotor); //Call motor control periodicly
NKarandey 46:5c50778bb2d5 263
NKarandey 46:5c50778bb2d5 264 CHA.rise(&CHA_rise);
NKarandey 46:5c50778bb2d5 265 CHA.fall(&CHA_fall);
NKarandey 41:d52445129908 266 CHB.rise(&CHB_rise);
NKarandey 41:d52445129908 267 CHB.fall(&CHB_fall);
NKarandey 41:d52445129908 268
davidanderle 47:49e6c3fb25dc 269 motorOut(4, 0.4f); //Kickstart the motor
davidanderle 47:49e6c3fb25dc 270 controlTicker.attach(&controlVelocity, dtControl);
davidanderle 47:49e6c3fb25dc 271 // precisionMotorOut(360-120); //Kickstart motor with 120deg
NKarandey 46:5c50778bb2d5 272 // pwmPeriod.attach(&velocityPwmPeriod, velocityPeriod);
NKarandey 46:5c50778bb2d5 273
davidanderle 47:49e6c3fb25dc 274 // serialOutputer.start(callback(serialOut));
davidanderle 47:49e6c3fb25dc 275 // serialOutputer.set_priority(osPriorityLow);
davidanderle 47:49e6c3fb25dc 276 // serialInputer.start(callback(serialIn));
davidanderle 47:49e6c3fb25dc 277 // serialInputer.set_priority(osPriorityLow);
NKarandey 46:5c50778bb2d5 278
NKarandey 44:0b92f72641d7 279 while (true) {
davidanderle 47:49e6c3fb25dc 280 // Thread::wait(1000);
davidanderle 47:49e6c3fb25dc 281 // motorOut((state-orState+lead+6)%6, velocityDuty);
davidanderle 47:49e6c3fb25dc 282 pc.printf("Speed: %f, duty cycle: %f, currentRevs: %i, vErr %f, dErr %f, totalVErr: %f \n\r",w3, velocityDuty, count_i3, vErr, dErr, totalVErr);
davidanderle 47:49e6c3fb25dc 283 wait(0.1);
davidanderle 47:49e6c3fb25dc 284 // if(currentRevs >= goalRevs){
davidanderle 47:49e6c3fb25dc 285 // controlTicker.detach();
davidanderle 47:49e6c3fb25dc 286 //// pwmPeriod.detach();
davidanderle 47:49e6c3fb25dc 287 // stopInterrupts();
davidanderle 47:49e6c3fb25dc 288 // stopMotor(/*(int)diskPosition*/);
davidanderle 47:49e6c3fb25dc 289 // }
NKarandey 46:5c50778bb2d5 290
NKarandey 41:d52445129908 291 }
NKarandey 41:d52445129908 292 return 0;
NKarandey 43:8b6b4040e635 293 }