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

Committer:
dkp14
Date:
Fri Mar 17 20:51:37 2017 +0000
Revision:
39:ae41a212c170
Parent:
38:e57ffbd9b75d
Child:
40:04a05851ae7b
fixed the rotation counter - it doesn't count backwards rotations as a rotation anymore

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