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@43:8b6b4040e635, 2017-03-18 (annotated)

Committer:
NKarandey
Date:
Sat Mar 18 15:59:37 2017 +0000
Revision:
43:8b6b4040e635
Parent:
42:2ee563cd6223
Child:
44:0b92f72641d7
Implemented multi-threaded operation with serial input



Modified parser to return some variables rather than just bool

Extended the main loop with runCommand that will make the motor

do what the command does. Currently these functions are empty.



The main file should still appear to be doing the same thing as in previous

version.

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 43:8b6b4040e635 22 void rotate(float r) {}
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 41:d52445129908 51
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
davidanderle 42:2ee563cd6223 61 #define dt 0.002f //given in ms,used to call the PD c.
NKarandey 41:d52445129908 62
NKarandey 41:d52445129908 63 void control(){
davidanderle 42:2ee563cd6223 64 if (w3 > 300) { //restrict the motor speed to 300
davidanderle 42:2ee563cd6223 65 lead = 2; // rad/s
NKarandey 41:d52445129908 66 return;
NKarandey 41:d52445129908 67 }
NKarandey 41:d52445129908 68 prevError = currentError;
davidanderle 42:2ee563cd6223 69 currentRevs = diskPosition / 360 + count_i3; // angle/360 + #of revs
davidanderle 42:2ee563cd6223 70 currentError = goalRevs - currentRevs; //P term
davidanderle 42:2ee563cd6223 71 dError = (currentError - prevError)/dt; //D term
davidanderle 42:2ee563cd6223 72 duty = k*(kp*currentError + kd*dError); //Control motor duty
NKarandey 41:d52445129908 73 if (duty > 0) lead = -2;
davidanderle 42:2ee563cd6223 74 else { //if duty < 0, reverse motor spin
davidanderle 42:2ee563cd6223 75 lead = 2; // direction to decelerate
NKarandey 41:d52445129908 76 duty = -duty;
NKarandey 41:d52445129908 77 }
NKarandey 41:d52445129908 78 }
NKarandey 41:d52445129908 79
NKarandey 41:d52445129908 80 void i1rise(){
NKarandey 41:d52445129908 81 state = updateState();
NKarandey 41:d52445129908 82 motorOut((state-orState+lead+6)%6, duty);
NKarandey 41:d52445129908 83
davidanderle 42:2ee563cd6223 84 if (I3.read() == 1) { //Only count revolutions if the
davidanderle 42:2ee563cd6223 85 count_i3++; // rotor spins forward
NKarandey 41:d52445129908 86 }
NKarandey 41:d52445129908 87 }
davidanderle 42:2ee563cd6223 88 //TODO merge with i_edge by measuring angular velocity in i1rise.
NKarandey 41:d52445129908 89 void i3rise(){
NKarandey 41:d52445129908 90 state = updateState();
NKarandey 41:d52445129908 91 motorOut((state-orState+lead+6)%6, duty);
NKarandey 41:d52445129908 92
davidanderle 42:2ee563cd6223 93 w3 = angle/dt_I3.read(); //Calc angular velocity
NKarandey 41:d52445129908 94
NKarandey 41:d52445129908 95 dt_I3.reset();
NKarandey 41:d52445129908 96 }
NKarandey 41:d52445129908 97
davidanderle 42:2ee563cd6223 98 void i_edge(){ //Upon status led interrupt, update
davidanderle 42:2ee563cd6223 99 state = updateState(); // the motor output
NKarandey 41:d52445129908 100 motorOut((state-orState+lead+6)%6, duty);
NKarandey 41:d52445129908 101 }
davidanderle 42:2ee563cd6223 102 //Todo: add comments on this fucntion
NKarandey 41:d52445129908 103 void updateDiskPosition() {
NKarandey 41:d52445129908 104 if (CH_state != CH_state_prev) {
NKarandey 41:d52445129908 105 int diff = CH_state - CH_state_prev;
NKarandey 41:d52445129908 106
NKarandey 41:d52445129908 107 CH_state_prev = CH_state;
NKarandey 41:d52445129908 108 if (abs(diff) == 1 || abs(diff) == 3) {
NKarandey 41:d52445129908 109 if (diff < 0)
NKarandey 41:d52445129908 110 diskPosition += angularResolution;
NKarandey 41:d52445129908 111 else
NKarandey 41:d52445129908 112 diskPosition -= angularResolution;
NKarandey 41:d52445129908 113 }
NKarandey 41:d52445129908 114 else if (abs(diff) == 2) {
NKarandey 41:d52445129908 115 if (diff < 0)
NKarandey 41:d52445129908 116 diskPosition += 2.0f * angularResolution;
NKarandey 41:d52445129908 117 else
NKarandey 41:d52445129908 118 diskPosition -= 2.0f * angularResolution;
NKarandey 41:d52445129908 119 }
NKarandey 41:d52445129908 120
NKarandey 41:d52445129908 121 if (diskPosition >= 360.0f) {
NKarandey 41:d52445129908 122 diskPosition -= 360.0f;
NKarandey 41:d52445129908 123 } else if (diskPosition < -360.0f) {
NKarandey 41:d52445129908 124 diskPosition += 360.0f;
NKarandey 41:d52445129908 125 }
NKarandey 41:d52445129908 126 }
NKarandey 41:d52445129908 127 }
NKarandey 41:d52445129908 128
NKarandey 41:d52445129908 129 void updateRelativeState() {
NKarandey 41:d52445129908 130 CH_state = relativeStateMap[CHB_state + 2*CHA_state];
NKarandey 41:d52445129908 131 }
NKarandey 41:d52445129908 132
NKarandey 41:d52445129908 133 void CHA_rise() {
NKarandey 41:d52445129908 134 CHA_state = 1;
NKarandey 41:d52445129908 135 updateRelativeState();
NKarandey 41:d52445129908 136 updateDiskPosition();
NKarandey 41:d52445129908 137 }
NKarandey 41:d52445129908 138 void CHA_fall() {
NKarandey 41:d52445129908 139 CHA_state = 0;
NKarandey 41:d52445129908 140 updateRelativeState();
NKarandey 41:d52445129908 141 updateDiskPosition();
NKarandey 41:d52445129908 142 }
NKarandey 41:d52445129908 143 void CHB_rise() {
NKarandey 41:d52445129908 144 CHB_state = 1;
NKarandey 41:d52445129908 145 updateRelativeState();
NKarandey 41:d52445129908 146 updateDiskPosition();
NKarandey 41:d52445129908 147 }
NKarandey 41:d52445129908 148 void CHB_fall() {
NKarandey 41:d52445129908 149 CHB_state = 0;
NKarandey 41:d52445129908 150 updateRelativeState();
NKarandey 41:d52445129908 151 updateDiskPosition();
NKarandey 41:d52445129908 152 }
NKarandey 41:d52445129908 153
NKarandey 43:8b6b4040e635 154 void runCommand(const ParseResult& command);
NKarandey 43:8b6b4040e635 155
NKarandey 41:d52445129908 156 int main() {
NKarandey 43:8b6b4040e635 157 Thread serialInput;
NKarandey 43:8b6b4040e635 158 serialInput.start(callback(serialThread));
NKarandey 41:d52445129908 159 motorHome(); //Initialise motor before any interrupt
NKarandey 41:d52445129908 160
NKarandey 41:d52445129908 161 dt_I3.start(); //Start the time counters for velocity
NKarandey 41:d52445129908 162
davidanderle 42:2ee563cd6223 163 controlTicker.attach(&control, dt); //Adjust position every 2ms
NKarandey 41:d52445129908 164
NKarandey 41:d52445129908 165 I1.rise(&i1rise); //Assign interrupt handlers for LEDs
NKarandey 41:d52445129908 166 I1.fall(&i_edge);
NKarandey 41:d52445129908 167 I2.rise(&i_edge);
NKarandey 41:d52445129908 168 I2.fall(&i_edge);
NKarandey 41:d52445129908 169 I3.rise(&i3rise);
NKarandey 41:d52445129908 170 I3.fall(&i_edge);
NKarandey 41:d52445129908 171
davidanderle 42:2ee563cd6223 172 CHA.rise(&CHA_rise); //Assign interrupt handlers for
davidanderle 42:2ee563cd6223 173 CHA.fall(&CHA_fall); // precision angle LEDs
NKarandey 41:d52445129908 174 CHB.rise(&CHB_rise);
NKarandey 41:d52445129908 175 CHB.fall(&CHB_fall);
NKarandey 41:d52445129908 176
davidanderle 42:2ee563cd6223 177 state = updateState(); //Kickstart the motor
NKarandey 41:d52445129908 178 motorTimer.start();
davidanderle 42:2ee563cd6223 179 motorOut((state-orState+lead+6)%6, 0.3f);
davidanderle 42:2ee563cd6223 180
NKarandey 41:d52445129908 181 while (count_i3 <= goalRevs+1) {
NKarandey 41:d52445129908 182 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 43:8b6b4040e635 183 // Work with user input here
NKarandey 43:8b6b4040e635 184 if (commandReady) {
NKarandey 43:8b6b4040e635 185 printf("Got command: %d\n\r", parseResult.mode);
NKarandey 43:8b6b4040e635 186 commandReady = false;
NKarandey 43:8b6b4040e635 187 readyForCommand = false;
NKarandey 43:8b6b4040e635 188 runCommand(parseResult);
NKarandey 43:8b6b4040e635 189 readyForCommand = true;
NKarandey 43:8b6b4040e635 190 }
NKarandey 41:d52445129908 191 }
davidanderle 42:2ee563cd6223 192 stopMotor(); //Stop the motor if position is reached
NKarandey 41:d52445129908 193 return 0;
NKarandey 43:8b6b4040e635 194 }
NKarandey 43:8b6b4040e635 195
NKarandey 43:8b6b4040e635 196 void runCommand(const ParseResult& command) {
NKarandey 43:8b6b4040e635 197 int mode = command.mode;
NKarandey 43:8b6b4040e635 198 switch(mode) {
NKarandey 43:8b6b4040e635 199 case 0: rotateWith(command.rotations, command.velocity); break;
NKarandey 43:8b6b4040e635 200 case 1: rotate(command.rotations); break;
NKarandey 43:8b6b4040e635 201 case 2: setVelocity(command.velocity); break;
NKarandey 43:8b6b4040e635 202 case 3: playTunes(command.tunes); break;
NKarandey 43:8b6b4040e635 203 default: return;
NKarandey 43:8b6b4040e635 204 }
NKarandey 43:8b6b4040e635 205 }
NKarandey 43:8b6b4040e635 206