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@42:2ee563cd6223, 2017-03-17 (annotated)

Committer:
davidanderle
Date:
Fri Mar 17 23:17:42 2017 +0000
Revision:
42:2ee563cd6223
Parent:
41:d52445129908
Child:
43:8b6b4040e635
Cleaning + comments and additional TODOs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NKarandey 41:d52445129908 1 #include "mbed.h"
NKarandey 41:d52445129908 2 #include "rtos.h"
NKarandey 41:d52445129908 3 #include "definitions.h"
NKarandey 41:d52445129908 4 #include "motorControl.h"
NKarandey 41:d52445129908 5 #include "parser.h"
NKarandey 41:d52445129908 6 #include <cmath>
NKarandey 41:d52445129908 7
NKarandey 41:d52445129908 8 volatile float w3 = 0; //Angular velocity
NKarandey 41:d52445129908 9 volatile float duty = 0.5;
NKarandey 41:d52445129908 10 volatile int count_i3 = 0;
NKarandey 41:d52445129908 11
NKarandey 41:d52445129908 12 volatile int CHA_state = 0x00;
NKarandey 41:d52445129908 13 volatile int CHB_state = 0x00;
NKarandey 41:d52445129908 14 volatile int CH_state = 0x00;
NKarandey 41:d52445129908 15 volatile int CH_state_prev = 0x00;
NKarandey 41:d52445129908 16
davidanderle 42:2ee563cd6223 17 volatile float diskPosition = 0.0; //in degrees
NKarandey 41:d52445129908 18
NKarandey 41:d52445129908 19 Timer dt_I3;
NKarandey 41:d52445129908 20 Timer motorTimer;
NKarandey 41:d52445129908 21 Ticker controlTicker;
NKarandey 41:d52445129908 22
davidanderle 42:2ee563cd6223 23 volatile float currentRevs = 0.0; //number of revs done
davidanderle 42:2ee563cd6223 24 volatile float goalRevs = 10.0; //number of revs to do
davidanderle 42:2ee563cd6223 25 volatile float prevError = 0.0; //previous error in position
davidanderle 42:2ee563cd6223 26 volatile double dError = 0.0;
davidanderle 42:2ee563cd6223 27 volatile float currentError = 0.0; //current error in position
NKarandey 41:d52445129908 28
NKarandey 41:d52445129908 29 #define kp 0.012f
davidanderle 42:2ee563cd6223 30 #define kd 0.02f
NKarandey 41:d52445129908 31 #define k 10.0f
davidanderle 42:2ee563cd6223 32 #define dt 0.002f //given in ms,used to call the PD c.
NKarandey 41:d52445129908 33
NKarandey 41:d52445129908 34 void control(){
davidanderle 42:2ee563cd6223 35 if (w3 > 300) { //restrict the motor speed to 300
davidanderle 42:2ee563cd6223 36 lead = 2; // rad/s
NKarandey 41:d52445129908 37 return;
NKarandey 41:d52445129908 38 }
NKarandey 41:d52445129908 39 prevError = currentError;
davidanderle 42:2ee563cd6223 40 currentRevs = diskPosition / 360 + count_i3; // angle/360 + #of revs
davidanderle 42:2ee563cd6223 41 currentError = goalRevs - currentRevs; //P term
davidanderle 42:2ee563cd6223 42 dError = (currentError - prevError)/dt; //D term
davidanderle 42:2ee563cd6223 43 duty = k*(kp*currentError + kd*dError); //Control motor duty
NKarandey 41:d52445129908 44 if (duty > 0) lead = -2;
davidanderle 42:2ee563cd6223 45 else { //if duty < 0, reverse motor spin
davidanderle 42:2ee563cd6223 46 lead = 2; // direction to decelerate
NKarandey 41:d52445129908 47 duty = -duty;
NKarandey 41:d52445129908 48 }
NKarandey 41:d52445129908 49 }
NKarandey 41:d52445129908 50
NKarandey 41:d52445129908 51 void i1rise(){
NKarandey 41:d52445129908 52 state = updateState();
NKarandey 41:d52445129908 53 motorOut((state-orState+lead+6)%6, duty);
NKarandey 41:d52445129908 54
davidanderle 42:2ee563cd6223 55 if (I3.read() == 1) { //Only count revolutions if the
davidanderle 42:2ee563cd6223 56 count_i3++; // rotor spins forward
NKarandey 41:d52445129908 57 }
NKarandey 41:d52445129908 58 }
davidanderle 42:2ee563cd6223 59 //TODO merge with i_edge by measuring angular velocity in i1rise.
NKarandey 41:d52445129908 60 void i3rise(){
NKarandey 41:d52445129908 61 state = updateState();
NKarandey 41:d52445129908 62 motorOut((state-orState+lead+6)%6, duty);
NKarandey 41:d52445129908 63
davidanderle 42:2ee563cd6223 64 w3 = angle/dt_I3.read(); //Calc angular velocity
NKarandey 41:d52445129908 65
NKarandey 41:d52445129908 66 dt_I3.reset();
NKarandey 41:d52445129908 67 }
NKarandey 41:d52445129908 68
davidanderle 42:2ee563cd6223 69 void i_edge(){ //Upon status led interrupt, update
davidanderle 42:2ee563cd6223 70 state = updateState(); // the motor output
NKarandey 41:d52445129908 71 motorOut((state-orState+lead+6)%6, duty);
NKarandey 41:d52445129908 72 }
davidanderle 42:2ee563cd6223 73 //Todo: add comments on this fucntion
NKarandey 41:d52445129908 74 void updateDiskPosition() {
NKarandey 41:d52445129908 75 if (CH_state != CH_state_prev) {
NKarandey 41:d52445129908 76 int diff = CH_state - CH_state_prev;
NKarandey 41:d52445129908 77
NKarandey 41:d52445129908 78 CH_state_prev = CH_state;
NKarandey 41:d52445129908 79 if (abs(diff) == 1 || abs(diff) == 3) {
NKarandey 41:d52445129908 80 if (diff < 0)
NKarandey 41:d52445129908 81 diskPosition += angularResolution;
NKarandey 41:d52445129908 82 else
NKarandey 41:d52445129908 83 diskPosition -= angularResolution;
NKarandey 41:d52445129908 84 }
NKarandey 41:d52445129908 85 else if (abs(diff) == 2) {
NKarandey 41:d52445129908 86 if (diff < 0)
NKarandey 41:d52445129908 87 diskPosition += 2.0f * angularResolution;
NKarandey 41:d52445129908 88 else
NKarandey 41:d52445129908 89 diskPosition -= 2.0f * angularResolution;
NKarandey 41:d52445129908 90 }
NKarandey 41:d52445129908 91
NKarandey 41:d52445129908 92 if (diskPosition >= 360.0f) {
NKarandey 41:d52445129908 93 diskPosition -= 360.0f;
NKarandey 41:d52445129908 94 } else if (diskPosition < -360.0f) {
NKarandey 41:d52445129908 95 diskPosition += 360.0f;
NKarandey 41:d52445129908 96 }
NKarandey 41:d52445129908 97 }
NKarandey 41:d52445129908 98 }
NKarandey 41:d52445129908 99
NKarandey 41:d52445129908 100 void updateRelativeState() {
NKarandey 41:d52445129908 101 CH_state = relativeStateMap[CHB_state + 2*CHA_state];
NKarandey 41:d52445129908 102 }
NKarandey 41:d52445129908 103
NKarandey 41:d52445129908 104 void CHA_rise() {
NKarandey 41:d52445129908 105 CHA_state = 1;
NKarandey 41:d52445129908 106 updateRelativeState();
NKarandey 41:d52445129908 107 updateDiskPosition();
NKarandey 41:d52445129908 108 }
NKarandey 41:d52445129908 109 void CHA_fall() {
NKarandey 41:d52445129908 110 CHA_state = 0;
NKarandey 41:d52445129908 111 updateRelativeState();
NKarandey 41:d52445129908 112 updateDiskPosition();
NKarandey 41:d52445129908 113 }
NKarandey 41:d52445129908 114 void CHB_rise() {
NKarandey 41:d52445129908 115 CHB_state = 1;
NKarandey 41:d52445129908 116 updateRelativeState();
NKarandey 41:d52445129908 117 updateDiskPosition();
NKarandey 41:d52445129908 118 }
NKarandey 41:d52445129908 119 void CHB_fall() {
NKarandey 41:d52445129908 120 CHB_state = 0;
NKarandey 41:d52445129908 121 updateRelativeState();
NKarandey 41:d52445129908 122 updateDiskPosition();
NKarandey 41:d52445129908 123 }
NKarandey 41:d52445129908 124
NKarandey 41:d52445129908 125 int main() {
NKarandey 41:d52445129908 126 motorHome(); //Initialise motor before any interrupt
NKarandey 41:d52445129908 127
NKarandey 41:d52445129908 128 dt_I3.start(); //Start the time counters for velocity
NKarandey 41:d52445129908 129
davidanderle 42:2ee563cd6223 130 controlTicker.attach(&control, dt); //Adjust position every 2ms
NKarandey 41:d52445129908 131
NKarandey 41:d52445129908 132 I1.rise(&i1rise); //Assign interrupt handlers for LEDs
NKarandey 41:d52445129908 133 I1.fall(&i_edge);
NKarandey 41:d52445129908 134 I2.rise(&i_edge);
NKarandey 41:d52445129908 135 I2.fall(&i_edge);
NKarandey 41:d52445129908 136 I3.rise(&i3rise);
NKarandey 41:d52445129908 137 I3.fall(&i_edge);
NKarandey 41:d52445129908 138
davidanderle 42:2ee563cd6223 139 CHA.rise(&CHA_rise); //Assign interrupt handlers for
davidanderle 42:2ee563cd6223 140 CHA.fall(&CHA_fall); // precision angle LEDs
NKarandey 41:d52445129908 141 CHB.rise(&CHB_rise);
NKarandey 41:d52445129908 142 CHB.fall(&CHB_fall);
NKarandey 41:d52445129908 143
davidanderle 42:2ee563cd6223 144 state = updateState(); //Kickstart the motor
NKarandey 41:d52445129908 145 motorTimer.start();
davidanderle 42:2ee563cd6223 146 motorOut((state-orState+lead+6)%6, 0.3f);
davidanderle 42:2ee563cd6223 147
NKarandey 41:d52445129908 148 while (count_i3 <= goalRevs+1) {
NKarandey 41:d52445129908 149 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 41:d52445129908 150 }
davidanderle 42:2ee563cd6223 151 stopMotor(); //Stop the motor if position is reached
NKarandey 41:d52445129908 152 return 0;
NKarandey 41:d52445129908 153 }