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@36:747d80e3aca9, 2017-03-17 (annotated)

Committer:
dkp14
Date:
Fri Mar 17 02:13:10 2017 +0000
Revision:
36:747d80e3aca9
Parent:
35:5857105c9956
Child:
37:bf96972df861
precision control moved to main, PID tuned

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