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

Committer:
dkp14
Date:
Fri Mar 17 19:24:08 2017 +0000
Revision:
37:bf96972df861
Parent:
36:747d80e3aca9
Child:
38:e57ffbd9b75d
minor changes

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