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@44:0b92f72641d7, 2017-03-18 (annotated)

Committer:
NKarandey
Date:
Sat Mar 18 21:35:45 2017 +0000
Revision:
44:0b92f72641d7
Parent:
43:8b6b4040e635
Child:
45:bfd7cbd41957
Motor spins only in response to user commands.; ; For now only have rotation specified number of revolutions

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 43:8b6b4040e635 10 Mutex mutex;
NKarandey 41:d52445129908 11
NKarandey 41:d52445129908 12 volatile float w3 = 0; //Angular velocity
NKarandey 41:d52445129908 13 volatile float duty = 0.5;
NKarandey 41:d52445129908 14 volatile int count_i3 = 0;
NKarandey 41:d52445129908 15
NKarandey 43:8b6b4040e635 16 volatile char userInput[256];
NKarandey 43:8b6b4040e635 17 volatile bool commandReady = false;
NKarandey 43:8b6b4040e635 18 ParseResult parseResult;
NKarandey 43:8b6b4040e635 19 volatile bool readyForCommand = true;
NKarandey 43:8b6b4040e635 20
NKarandey 43:8b6b4040e635 21 void rotateWith(float r, float v) {}
NKarandey 44:0b92f72641d7 22
NKarandey 43:8b6b4040e635 23 void setVelocity(float v) {}
NKarandey 43:8b6b4040e635 24 void playTunes(const vector<float>& tunes) {}
NKarandey 43:8b6b4040e635 25
NKarandey 43:8b6b4040e635 26 void serialThread() {
NKarandey 43:8b6b4040e635 27 while(true) {
NKarandey 43:8b6b4040e635 28 if (readyForCommand) {
NKarandey 43:8b6b4040e635 29 scanf("%s", userInput);
NKarandey 43:8b6b4040e635 30 ParseResult curr = parse((char *) userInput);
NKarandey 43:8b6b4040e635 31 if (curr.success) {
NKarandey 43:8b6b4040e635 32 mutex.lock();
NKarandey 43:8b6b4040e635 33 commandReady = true;
NKarandey 43:8b6b4040e635 34 parseResult = curr;
NKarandey 43:8b6b4040e635 35 mutex.unlock();
NKarandey 43:8b6b4040e635 36 }
NKarandey 43:8b6b4040e635 37 }
NKarandey 43:8b6b4040e635 38 }
NKarandey 43:8b6b4040e635 39 }
NKarandey 43:8b6b4040e635 40
NKarandey 41:d52445129908 41 volatile int CHA_state = 0x00;
NKarandey 41:d52445129908 42 volatile int CHB_state = 0x00;
NKarandey 41:d52445129908 43 volatile int CH_state = 0x00;
NKarandey 41:d52445129908 44 volatile int CH_state_prev = 0x00;
NKarandey 41:d52445129908 45
davidanderle 42:2ee563cd6223 46 volatile float diskPosition = 0.0; //in degrees
NKarandey 41:d52445129908 47
NKarandey 41:d52445129908 48 Timer dt_I3;
NKarandey 41:d52445129908 49 Timer motorTimer;
NKarandey 41:d52445129908 50 Ticker controlTicker;
NKarandey 44:0b92f72641d7 51 Ticker motorOutTicker;
davidanderle 42:2ee563cd6223 52 volatile float currentRevs = 0.0; //number of revs done
davidanderle 42:2ee563cd6223 53 volatile float goalRevs = 10.0; //number of revs to do
davidanderle 42:2ee563cd6223 54 volatile float prevError = 0.0; //previous error in position
davidanderle 42:2ee563cd6223 55 volatile double dError = 0.0;
davidanderle 42:2ee563cd6223 56 volatile float currentError = 0.0; //current error in position
NKarandey 41:d52445129908 57
NKarandey 41:d52445129908 58 #define kp 0.012f
davidanderle 42:2ee563cd6223 59 #define kd 0.02f
NKarandey 41:d52445129908 60 #define k 10.0f
NKarandey 44:0b92f72641d7 61
NKarandey 44:0b92f72641d7 62 const float VKp = 0.5f;
NKarandey 44:0b92f72641d7 63 const float VKi = 0.0f;
NKarandey 44:0b92f72641d7 64 const float VKd = 0.0f;
NKarandey 44:0b92f72641d7 65 volatile float vPrevError = 0.0f;
NKarandey 44:0b92f72641d7 66 volatile float targetV = 300.0f;
NKarandey 44:0b92f72641d7 67 //given in ms,used to call the PD controller
NKarandey 44:0b92f72641d7 68 const float dtControl = 0.002f;
NKarandey 44:0b92f72641d7 69
NKarandey 44:0b92f72641d7 70 // Period of giving power to the motor
NKarandey 44:0b92f72641d7 71 const float dtMotor = 0.005f;
NKarandey 41:d52445129908 72
NKarandey 44:0b92f72641d7 73 volatile bool commandFinished = false;
NKarandey 44:0b92f72641d7 74
NKarandey 44:0b92f72641d7 75 void runMotor() {
NKarandey 44:0b92f72641d7 76 motorOut((state-orState+lead+6)%6, duty);
NKarandey 44:0b92f72641d7 77 }
NKarandey 44:0b92f72641d7 78
NKarandey 44:0b92f72641d7 79 void controlVelocity() {
NKarandey 44:0b92f72641d7 80 float vErr =
NKarandey 44:0b92f72641d7 81 }
NKarandey 44:0b92f72641d7 82
NKarandey 44:0b92f72641d7 83 void controlPosition(){
davidanderle 42:2ee563cd6223 84 if (w3 > 300) { //restrict the motor speed to 300
davidanderle 42:2ee563cd6223 85 lead = 2; // rad/s
NKarandey 41:d52445129908 86 return;
NKarandey 41:d52445129908 87 }
NKarandey 41:d52445129908 88 prevError = currentError;
davidanderle 42:2ee563cd6223 89 currentRevs = diskPosition / 360 + count_i3; // angle/360 + #of revs
davidanderle 42:2ee563cd6223 90 currentError = goalRevs - currentRevs; //P term
NKarandey 44:0b92f72641d7 91 // if (currentError < 0.5f) {
NKarandey 44:0b92f72641d7 92 // printf("Reached final position: %f\n\r", currentRevs);
NKarandey 44:0b92f72641d7 93 // commandFinished = true;
NKarandey 44:0b92f72641d7 94 // return;
NKarandey 44:0b92f72641d7 95 // }
NKarandey 44:0b92f72641d7 96 dError = (currentError - prevError)/dtControl; //D term
davidanderle 42:2ee563cd6223 97 duty = k*(kp*currentError + kd*dError); //Control motor duty
NKarandey 44:0b92f72641d7 98 if (duty > 0) {
NKarandey 44:0b92f72641d7 99 lead = -2;
NKarandey 44:0b92f72641d7 100 } else { //if duty < 0, reverse motor spin
davidanderle 42:2ee563cd6223 101 lead = 2; // direction to decelerate
NKarandey 41:d52445129908 102 duty = -duty;
NKarandey 41:d52445129908 103 }
NKarandey 41:d52445129908 104 }
NKarandey 41:d52445129908 105
NKarandey 44:0b92f72641d7 106 void rotate(float r) {
NKarandey 44:0b92f72641d7 107 printf("Rotating for %f revolutions\n\r", r);
NKarandey 44:0b92f72641d7 108 goalRevs = r;
NKarandey 44:0b92f72641d7 109 state = updateState();
NKarandey 44:0b92f72641d7 110 // Kickstart
NKarandey 44:0b92f72641d7 111 motorOut((state-orState+lead+6)%6, 0.3f);
NKarandey 44:0b92f72641d7 112 controlTicker.attach(&controlPosition, dtControl);
NKarandey 44:0b92f72641d7 113 motorOutTicker.attach(&runMotor, dtMotor);
NKarandey 44:0b92f72641d7 114 }
NKarandey 44:0b92f72641d7 115
NKarandey 41:d52445129908 116 void i1rise(){
NKarandey 41:d52445129908 117 state = updateState();
NKarandey 44:0b92f72641d7 118 //motorOut((state-orState+lead+6)%6, duty);
NKarandey 41:d52445129908 119
davidanderle 42:2ee563cd6223 120 if (I3.read() == 1) { //Only count revolutions if the
davidanderle 42:2ee563cd6223 121 count_i3++; // rotor spins forward
NKarandey 41:d52445129908 122 }
NKarandey 41:d52445129908 123 }
davidanderle 42:2ee563cd6223 124 //TODO merge with i_edge by measuring angular velocity in i1rise.
NKarandey 41:d52445129908 125 void i3rise(){
NKarandey 41:d52445129908 126 state = updateState();
NKarandey 44:0b92f72641d7 127 // motorOut((state-orState+lead+6)%6, duty);
NKarandey 41:d52445129908 128
davidanderle 42:2ee563cd6223 129 w3 = angle/dt_I3.read(); //Calc angular velocity
NKarandey 41:d52445129908 130
NKarandey 41:d52445129908 131 dt_I3.reset();
NKarandey 41:d52445129908 132 }
NKarandey 41:d52445129908 133
davidanderle 42:2ee563cd6223 134 void i_edge(){ //Upon status led interrupt, update
davidanderle 42:2ee563cd6223 135 state = updateState(); // the motor output
NKarandey 44:0b92f72641d7 136 // motorOut((state-orState+lead+6)%6, duty);
NKarandey 41:d52445129908 137 }
davidanderle 42:2ee563cd6223 138 //Todo: add comments on this fucntion
NKarandey 41:d52445129908 139 void updateDiskPosition() {
NKarandey 41:d52445129908 140 if (CH_state != CH_state_prev) {
NKarandey 41:d52445129908 141 int diff = CH_state - CH_state_prev;
NKarandey 41:d52445129908 142
NKarandey 41:d52445129908 143 CH_state_prev = CH_state;
NKarandey 41:d52445129908 144 if (abs(diff) == 1 || abs(diff) == 3) {
NKarandey 41:d52445129908 145 if (diff < 0)
NKarandey 41:d52445129908 146 diskPosition += angularResolution;
NKarandey 41:d52445129908 147 else
NKarandey 41:d52445129908 148 diskPosition -= angularResolution;
NKarandey 41:d52445129908 149 }
NKarandey 41:d52445129908 150 else if (abs(diff) == 2) {
NKarandey 41:d52445129908 151 if (diff < 0)
NKarandey 41:d52445129908 152 diskPosition += 2.0f * angularResolution;
NKarandey 41:d52445129908 153 else
NKarandey 41:d52445129908 154 diskPosition -= 2.0f * angularResolution;
NKarandey 41:d52445129908 155 }
NKarandey 41:d52445129908 156
NKarandey 41:d52445129908 157 if (diskPosition >= 360.0f) {
NKarandey 41:d52445129908 158 diskPosition -= 360.0f;
NKarandey 41:d52445129908 159 } else if (diskPosition < -360.0f) {
NKarandey 41:d52445129908 160 diskPosition += 360.0f;
NKarandey 41:d52445129908 161 }
NKarandey 41:d52445129908 162 }
NKarandey 41:d52445129908 163 }
NKarandey 41:d52445129908 164
NKarandey 41:d52445129908 165 void updateRelativeState() {
NKarandey 41:d52445129908 166 CH_state = relativeStateMap[CHB_state + 2*CHA_state];
NKarandey 41:d52445129908 167 }
NKarandey 41:d52445129908 168
NKarandey 41:d52445129908 169 void CHA_rise() {
NKarandey 41:d52445129908 170 CHA_state = 1;
NKarandey 41:d52445129908 171 updateRelativeState();
NKarandey 41:d52445129908 172 updateDiskPosition();
NKarandey 41:d52445129908 173 }
NKarandey 41:d52445129908 174 void CHA_fall() {
NKarandey 41:d52445129908 175 CHA_state = 0;
NKarandey 41:d52445129908 176 updateRelativeState();
NKarandey 41:d52445129908 177 updateDiskPosition();
NKarandey 41:d52445129908 178 }
NKarandey 41:d52445129908 179 void CHB_rise() {
NKarandey 41:d52445129908 180 CHB_state = 1;
NKarandey 41:d52445129908 181 updateRelativeState();
NKarandey 41:d52445129908 182 updateDiskPosition();
NKarandey 41:d52445129908 183 }
NKarandey 41:d52445129908 184 void CHB_fall() {
NKarandey 41:d52445129908 185 CHB_state = 0;
NKarandey 41:d52445129908 186 updateRelativeState();
NKarandey 41:d52445129908 187 updateDiskPosition();
NKarandey 41:d52445129908 188 }
NKarandey 41:d52445129908 189
NKarandey 43:8b6b4040e635 190 void runCommand(const ParseResult& command);
NKarandey 43:8b6b4040e635 191
NKarandey 41:d52445129908 192 int main() {
NKarandey 43:8b6b4040e635 193 Thread serialInput;
NKarandey 43:8b6b4040e635 194 serialInput.start(callback(serialThread));
NKarandey 41:d52445129908 195 motorHome(); //Initialise motor before any interrupt
NKarandey 41:d52445129908 196
NKarandey 41:d52445129908 197 dt_I3.start(); //Start the time counters for velocity
NKarandey 41:d52445129908 198
NKarandey 41:d52445129908 199
NKarandey 41:d52445129908 200 I1.rise(&i1rise); //Assign interrupt handlers for LEDs
NKarandey 41:d52445129908 201 I1.fall(&i_edge);
NKarandey 41:d52445129908 202 I2.rise(&i_edge);
NKarandey 41:d52445129908 203 I2.fall(&i_edge);
NKarandey 41:d52445129908 204 I3.rise(&i3rise);
NKarandey 41:d52445129908 205 I3.fall(&i_edge);
NKarandey 41:d52445129908 206
davidanderle 42:2ee563cd6223 207 CHA.rise(&CHA_rise); //Assign interrupt handlers for
davidanderle 42:2ee563cd6223 208 CHA.fall(&CHA_fall); // precision angle LEDs
NKarandey 41:d52445129908 209 CHB.rise(&CHB_rise);
NKarandey 41:d52445129908 210 CHB.fall(&CHB_fall);
NKarandey 41:d52445129908 211
NKarandey 44:0b92f72641d7 212 // state = updateState();
NKarandey 44:0b92f72641d7 213 // // Kickstart
NKarandey 44:0b92f72641d7 214 // motorOut((state-orState+lead+6)%6, 0.3f);
NKarandey 44:0b92f72641d7 215 // controlTicker.attach(&controlPosition, dtControl);
NKarandey 44:0b92f72641d7 216 // motorOutTicker.attach(&runMotor, dtMotor);
NKarandey 44:0b92f72641d7 217 printf("Ready\n\r");
NKarandey 44:0b92f72641d7 218 while (true) {
NKarandey 44:0b92f72641d7 219 if (count_i3 >= goalRevs && !commandFinished) commandFinished = true;
NKarandey 44:0b92f72641d7 220 // pc.printf("Speed: %f, duty cycle: %f, revs done: %d, dError: %f , currentError: %f, prevError: %f, currentRevs: %f \n\r",w3, duty, count_i3, dError, currentError, prevError, currentRevs);
NKarandey 44:0b92f72641d7 221 // printf("dError: %f , currentError: %f, prevError: %f, currentRevs: %f\n\r",dError, currentError, prevError, currentRevs);
NKarandey 43:8b6b4040e635 222 // Work with user input here
NKarandey 43:8b6b4040e635 223 if (commandReady) {
NKarandey 44:0b92f72641d7 224 // printf("Got command: %d\n\r", parseResult.mode);
NKarandey 43:8b6b4040e635 225 commandReady = false;
NKarandey 43:8b6b4040e635 226 readyForCommand = false;
NKarandey 44:0b92f72641d7 227 commandFinished = false;
NKarandey 43:8b6b4040e635 228 runCommand(parseResult);
NKarandey 44:0b92f72641d7 229 }
NKarandey 44:0b92f72641d7 230 if (commandFinished && !readyForCommand) {
NKarandey 44:0b92f72641d7 231 motorOutTicker.detach();
NKarandey 44:0b92f72641d7 232 stopMotor(); //Turn off the motor if position is reached
NKarandey 43:8b6b4040e635 233 readyForCommand = true;
NKarandey 44:0b92f72641d7 234 printf("Ready\n\r");
NKarandey 43:8b6b4040e635 235 }
NKarandey 41:d52445129908 236 }
NKarandey 41:d52445129908 237 return 0;
NKarandey 43:8b6b4040e635 238 }
NKarandey 43:8b6b4040e635 239
NKarandey 43:8b6b4040e635 240 void runCommand(const ParseResult& command) {
NKarandey 43:8b6b4040e635 241 int mode = command.mode;
NKarandey 43:8b6b4040e635 242 switch(mode) {
NKarandey 43:8b6b4040e635 243 case 0: rotateWith(command.rotations, command.velocity); break;
NKarandey 43:8b6b4040e635 244 case 1: rotate(command.rotations); break;
NKarandey 43:8b6b4040e635 245 case 2: setVelocity(command.velocity); break;
NKarandey 43:8b6b4040e635 246 case 3: playTunes(command.tunes); break;
NKarandey 43:8b6b4040e635 247 default: return;
NKarandey 43:8b6b4040e635 248 }
NKarandey 43:8b6b4040e635 249 }
NKarandey 43:8b6b4040e635 250