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@38:e57ffbd9b75d, 2017-03-17 (annotated)

Committer:
dkp14
Date:
Fri Mar 17 20:27:50 2017 +0000
Revision:
38:e57ffbd9b75d
Parent:
37:bf96972df861
Child:
39:ae41a212c170
PID tuning works almost perfectly; maximum speed limited at 300; bug fixed with precise position calculation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dkp14 0:74a5723d604a 1 #include "mbed.h"
dkp14 0:74a5723d604a 2 #include "rtos.h"
dkp14 0:74a5723d604a 3 #include "definitions.h"
dkp14 0:74a5723d604a 4 #include "motorControl.h"
lb3314 15:b6025338e0eb 5 #include "parser.h"
dkp14 20:c3bdb8f73c02 6 #include <cmath>
dkp14 0:74a5723d604a 7
davidanderle 16:d4948633c559 8 volatile float w3 = 0; //Angular velocity
dkp14 21:dd4bbb617415 9 volatile float duty = 0.5;
dkp14 4:dc705df93090 10 volatile int count_i3 = 0;
dkp14 0:74a5723d604a 11
dkp14 36:747d80e3aca9 12 volatile int CHA_state = 0x00;
dkp14 36:747d80e3aca9 13 volatile int CHB_state = 0x00;
dkp14 36:747d80e3aca9 14 volatile int CH_state = 0x00;
dkp14 36:747d80e3aca9 15 volatile int CH_state_prev = 0x00;
dkp14 36:747d80e3aca9 16
dkp14 36:747d80e3aca9 17 volatile float diskPosition = 0.0; //in degrees
dkp14 36:747d80e3aca9 18
dkp14 0:74a5723d604a 19 Timer dt_I3;
dkp14 0:74a5723d604a 20 Timer motorTimer;
dkp14 4:dc705df93090 21 Ticker controlTicker;
dkp14 4:dc705df93090 22
dkp14 38:e57ffbd9b75d 23 volatile float currentRevs = 0.0; //number of revs done
dkp14 38:e57ffbd9b75d 24 volatile float goalRevs = 999.0;
dkp14 38:e57ffbd9b75d 25 volatile float prevError = 0.0;
dkp14 38:e57ffbd9b75d 26 volatile double dError = 0.0;
dkp14 38:e57ffbd9b75d 27 volatile float currentError = 0.0;
dkp14 4:dc705df93090 28
dkp14 38:e57ffbd9b75d 29 #define kp 0.009f
davidanderle 31:bfb4629ca327 30 #define ki 0.0f //0.05f
dkp14 38:e57ffbd9b75d 31 #define kd 0.025f //0.5f
dkp14 38:e57ffbd9b75d 32 #define k 10.0f
davidanderle 31:bfb4629ca327 33 #define dt 0.002f //given in ms, used to call the PID c.
dkp14 11:043a63c952a0 34
dkp14 4:dc705df93090 35 void control(){
dkp14 38:e57ffbd9b75d 36 if (w3 > 300) {
dkp14 38:e57ffbd9b75d 37 lead = 2;
dkp14 38:e57ffbd9b75d 38 return;
dkp14 38:e57ffbd9b75d 39 }
dkp14 38:e57ffbd9b75d 40 prevError = currentError;
dkp14 38:e57ffbd9b75d 41 currentRevs = diskPosition / 360 + count_i3; // 1/360degr + 2pi*revs
dkp14 38:e57ffbd9b75d 42 currentError = goalRevs - currentRevs;
dkp14 38:e57ffbd9b75d 43 dError = (currentError - prevError)/dt;
dkp14 38:e57ffbd9b75d 44 duty = k*(kp*currentError + kd*dError);
davidanderle 31:bfb4629ca327 45 if (duty > 0) lead = -2;
dkp14 37:bf96972df861 46 else {
dkp14 37:bf96972df861 47 lead = 2;
dkp14 37:bf96972df861 48 duty = -duty;
dkp14 37:bf96972df861 49 }
dkp14 0:74a5723d604a 50 }
dkp14 0:74a5723d604a 51
dkp14 0:74a5723d604a 52 void i3rise(){
dkp14 0:74a5723d604a 53 state = updateState();
davidanderle 2:fe637a5f3387 54 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 55
davidanderle 16:d4948633c559 56 w3 = angle/dt_I3.read(); //Calc angular velocity
dkp14 0:74a5723d604a 57
dkp14 0:74a5723d604a 58 dt_I3.reset();
dkp14 25:0ee6b164f234 59 count_i3++;
dkp14 0:74a5723d604a 60 }
dkp14 0:74a5723d604a 61
dkp14 11:043a63c952a0 62 void i_edge(){
dkp14 0:74a5723d604a 63 state = updateState();
davidanderle 2:fe637a5f3387 64 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 65 }
dkp14 36:747d80e3aca9 66
dkp14 36:747d80e3aca9 67 void updateDiskPosition() {
dkp14 36:747d80e3aca9 68 if (CH_state != CH_state_prev) {
dkp14 36:747d80e3aca9 69 int diff = CH_state - CH_state_prev;
dkp14 36:747d80e3aca9 70
dkp14 36:747d80e3aca9 71 CH_state_prev = CH_state;
dkp14 36:747d80e3aca9 72 if (abs(diff) == 1 || abs(diff) == 3) {
dkp14 36:747d80e3aca9 73 if (diff < 0)
dkp14 36:747d80e3aca9 74 diskPosition += angularResolution;
dkp14 36:747d80e3aca9 75 else
dkp14 36:747d80e3aca9 76 diskPosition -= angularResolution;
dkp14 36:747d80e3aca9 77 }
dkp14 36:747d80e3aca9 78 else if (abs(diff) == 2) {
dkp14 36:747d80e3aca9 79 if (diff < 0)
dkp14 36:747d80e3aca9 80 diskPosition += 2.0f * angularResolution;
dkp14 36:747d80e3aca9 81 else
dkp14 36:747d80e3aca9 82 diskPosition -= 2.0f * angularResolution;
dkp14 36:747d80e3aca9 83 }
dkp14 36:747d80e3aca9 84
dkp14 36:747d80e3aca9 85 if (diskPosition >= 360.0f) {
dkp14 36:747d80e3aca9 86 diskPosition -= 360.0f;
dkp14 36:747d80e3aca9 87 } else if (diskPosition < -360.0f) {
dkp14 36:747d80e3aca9 88 diskPosition += 360.0f;
dkp14 36:747d80e3aca9 89 }
dkp14 36:747d80e3aca9 90 }
dkp14 0:74a5723d604a 91 }
dkp14 36:747d80e3aca9 92
dkp14 36:747d80e3aca9 93 void updateRelativeState() {
dkp14 36:747d80e3aca9 94 CH_state = relativeStateMap[CHB_state + 2*CHA_state];
dkp14 36:747d80e3aca9 95 }
dkp14 36:747d80e3aca9 96
dkp14 36:747d80e3aca9 97 void CHA_rise() {
dkp14 36:747d80e3aca9 98 CHA_state = 1;
dkp14 36:747d80e3aca9 99 updateRelativeState();
dkp14 36:747d80e3aca9 100 updateDiskPosition();
dkp14 0:74a5723d604a 101 }
dkp14 36:747d80e3aca9 102 void CHA_fall() {
dkp14 36:747d80e3aca9 103 CHA_state = 0;
dkp14 36:747d80e3aca9 104 updateRelativeState();
dkp14 36:747d80e3aca9 105 updateDiskPosition();
dkp14 36:747d80e3aca9 106 }
dkp14 36:747d80e3aca9 107 void CHB_rise() {
dkp14 36:747d80e3aca9 108 CHB_state = 1;
dkp14 36:747d80e3aca9 109 updateRelativeState();
dkp14 36:747d80e3aca9 110 updateDiskPosition();
dkp14 36:747d80e3aca9 111 }
dkp14 36:747d80e3aca9 112 void CHB_fall() {
dkp14 36:747d80e3aca9 113 CHB_state = 0;
dkp14 36:747d80e3aca9 114 updateRelativeState();
dkp14 36:747d80e3aca9 115 updateDiskPosition();
dkp14 0:74a5723d604a 116 }
dkp14 0:74a5723d604a 117
dkp14 0:74a5723d604a 118 int main() {
davidanderle 16:d4948633c559 119 motorHome(); //Initialise motor before any interrupt
dkp14 0:74a5723d604a 120
davidanderle 16:d4948633c559 121 dt_I3.start(); //Start the time counters for velocity
dkp14 35:5857105c9956 122
dkp14 13:deb1e793f125 123 controlTicker.attach(&control, dt);
dkp14 0:74a5723d604a 124
davidanderle 16:d4948633c559 125 I1.rise(&i_edge); //Assign interrupt handlers for LEDs
dkp14 11:043a63c952a0 126 I1.fall(&i_edge);
dkp14 11:043a63c952a0 127 I2.rise(&i_edge);
dkp14 11:043a63c952a0 128 I2.fall(&i_edge);
dkp14 0:74a5723d604a 129 I3.rise(&i3rise);
dkp14 11:043a63c952a0 130 I3.fall(&i_edge);
dkp14 21:dd4bbb617415 131
dkp14 36:747d80e3aca9 132 CHA.rise(&CHA_rise);
dkp14 36:747d80e3aca9 133 CHA.fall(&CHA_fall);
dkp14 36:747d80e3aca9 134 CHB.rise(&CHB_rise);
dkp14 36:747d80e3aca9 135 CHB.fall(&CHB_fall);
dkp14 11:043a63c952a0 136
dkp14 10:25d8696cb2c6 137 state = updateState();
dkp14 11:043a63c952a0 138 motorTimer.start();
dkp14 25:0ee6b164f234 139 motorOut((state-orState+lead+6)%6, 0.3f); //Kickstart the motor
dkp14 38:e57ffbd9b75d 140 while (count_i3 <= goalRevs+1) {
dkp14 36:747d80e3aca9 141 //pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
dkp14 38:e57ffbd9b75d 142 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);
dkp14 36:747d80e3aca9 143 //pc.printf("Disk position: %f \n\r",fi0);
dkp14 0:74a5723d604a 144 }
dkp14 38:e57ffbd9b75d 145 stopMotor();
davidanderle 16:d4948633c559 146 return 0;
dkp14 0:74a5723d604a 147 }