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@48:02d5565b8fac, 2017-03-24 (annotated)

Committer:
davidanderle
Date:
Fri Mar 24 16:23:37 2017 +0000
Revision:
48:02d5565b8fac
Parent:
47:49e6c3fb25dc
Child:
49:731c95cd5852
asdfghj

Who changed what in which revision?

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