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@46:5c50778bb2d5, 2017-03-23 (annotated)

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