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@34:599634375ba0, 2017-03-16 (annotated)

Committer:
dkp14
Date:
Thu Mar 16 21:38:57 2017 +0000
Revision:
34:599634375ba0
Parent:
31:bfb4629ca327
Child:
35:5857105c9956
refactoring

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 0:74a5723d604a 12 Timer dt_I3;
dkp14 0:74a5723d604a 13 Timer motorTimer;
dkp14 4:dc705df93090 14 Ticker controlTicker;
dkp14 4:dc705df93090 15
davidanderle 16:d4948633c559 16 volatile float fi0 = 0; //number of revs done
dkp14 25:0ee6b164f234 17 volatile int goalRevs = 50;
dkp14 4:dc705df93090 18 volatile float fi = 2*3.1415*goalRevs;
dkp14 10:25d8696cb2c6 19 volatile float accError = 0;
dkp14 10:25d8696cb2c6 20 volatile float prevError = 0;
dkp14 4:dc705df93090 21
dkp14 11:043a63c952a0 22 float getDuty(float w){
davidanderle 16:d4948633c559 23 for (int i = 0; i < 28; i++) { //iterate through the angular velocities
dkp14 24:487c898c8d71 24 if (w >= angularVelocities[i] && w <= angularVelocities[i+1]) {
dkp14 11:043a63c952a0 25 if (w-angularVelocities[i] < angularVelocities[i+1]-w ) {
dkp14 11:043a63c952a0 26 return dutyCycles[i];
dkp14 11:043a63c952a0 27 }
dkp14 11:043a63c952a0 28 else {
dkp14 11:043a63c952a0 29 return dutyCycles[i+1];
dkp14 11:043a63c952a0 30 }
dkp14 11:043a63c952a0 31 }
dkp14 11:043a63c952a0 32 }
dkp14 21:dd4bbb617415 33 return dutyCycles[28];
dkp14 11:043a63c952a0 34 }
dkp14 11:043a63c952a0 35
davidanderle 31:bfb4629ca327 36 #define kp 0.0016f
davidanderle 31:bfb4629ca327 37 #define ki 0.0f //0.05f
davidanderle 31:bfb4629ca327 38 #define kd 0.00006f //0.5f
davidanderle 31:bfb4629ca327 39 #define dt 0.002f //given in ms, used to call the PID c.
davidanderle 31:bfb4629ca327 40
dkp14 4:dc705df93090 41 void control(){
davidanderle 16:d4948633c559 42 fi0 = 6.283 * count_i3; //fi0 = 2*pi*revs
dkp14 10:25d8696cb2c6 43 float error = fi - fi0;
dkp14 10:25d8696cb2c6 44 accError += error*dt;
dkp14 20:c3bdb8f73c02 45 float dError = (error - prevError)/dt;
davidanderle 31:bfb4629ca327 46 duty = kp*error + ki*accError + kd*dError;
dkp14 10:25d8696cb2c6 47 prevError = error;
davidanderle 31:bfb4629ca327 48 //if (abs(goalW) > w3) duty = 1 - w3/abs(goalW);
davidanderle 31:bfb4629ca327 49 //else duty = abs(goalW) / w3;
dkp14 25:0ee6b164f234 50 //duty = getDuty(abs(goalW));
davidanderle 31:bfb4629ca327 51 if (duty > 0) lead = -2;
dkp14 20:c3bdb8f73c02 52 else lead = 2;
dkp14 25:0ee6b164f234 53 //if (duty > 0) lead = -2;
dkp14 25:0ee6b164f234 54 //else lead = 2;
dkp14 0:74a5723d604a 55 }
dkp14 0:74a5723d604a 56
dkp14 0:74a5723d604a 57 void i3rise(){
dkp14 0:74a5723d604a 58 state = updateState();
davidanderle 2:fe637a5f3387 59 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 60
davidanderle 16:d4948633c559 61 w3 = angle/dt_I3.read(); //Calc angular velocity
dkp14 0:74a5723d604a 62
dkp14 0:74a5723d604a 63 dt_I3.reset();
dkp14 25:0ee6b164f234 64 count_i3++;
dkp14 0:74a5723d604a 65 }
dkp14 0:74a5723d604a 66
dkp14 11:043a63c952a0 67 void i_edge(){
dkp14 0:74a5723d604a 68 state = updateState();
davidanderle 2:fe637a5f3387 69 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 70 }
dkp14 0:74a5723d604a 71
dkp14 0:74a5723d604a 72 void CHA_rise(){
dkp14 0:74a5723d604a 73 }
dkp14 0:74a5723d604a 74 void CHA_fall(){
dkp14 0:74a5723d604a 75 }
dkp14 0:74a5723d604a 76 void CHB_rise(){
dkp14 0:74a5723d604a 77 }
dkp14 0:74a5723d604a 78 void CHB_fall(){
dkp14 0:74a5723d604a 79 }
dkp14 0:74a5723d604a 80
dkp14 0:74a5723d604a 81 int main() {
davidanderle 16:d4948633c559 82 motorHome(); //Initialise motor before any interrupt
dkp14 0:74a5723d604a 83
davidanderle 16:d4948633c559 84 dt_I3.start(); //Start the time counters for velocity
dkp14 21:dd4bbb617415 85
dkp14 13:deb1e793f125 86 controlTicker.attach(&control, dt);
dkp14 0:74a5723d604a 87
davidanderle 16:d4948633c559 88 I1.rise(&i_edge); //Assign interrupt handlers for LEDs
dkp14 11:043a63c952a0 89 I1.fall(&i_edge);
dkp14 11:043a63c952a0 90 I2.rise(&i_edge);
dkp14 11:043a63c952a0 91 I2.fall(&i_edge);
dkp14 0:74a5723d604a 92 I3.rise(&i3rise);
dkp14 11:043a63c952a0 93 I3.fall(&i_edge);
dkp14 21:dd4bbb617415 94
dkp14 0:74a5723d604a 95 // CHA.rise(&CHA_rise);
dkp14 0:74a5723d604a 96 // CHA.fall(&CHA_fall);
dkp14 0:74a5723d604a 97 // CHB.rise(&CHB_rise);
dkp14 0:74a5723d604a 98 // CHB.fall(&CHB_fall);
dkp14 11:043a63c952a0 99
dkp14 10:25d8696cb2c6 100 state = updateState();
dkp14 11:043a63c952a0 101 motorTimer.start();
dkp14 25:0ee6b164f234 102 motorOut((state-orState+lead+6)%6, 0.3f); //Kickstart the motor
dkp14 34:599634375ba0 103
davidanderle 16:d4948633c559 104 while (count_i3 <= goalRevs) {
dkp14 34:599634375ba0 105 pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
dkp14 0:74a5723d604a 106 }
dkp14 25:0ee6b164f234 107 while (abs(w3) > 10){
dkp14 34:599634375ba0 108 pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
dkp14 20:c3bdb8f73c02 109 }
dkp14 21:dd4bbb617415 110 stopMotor();
dkp14 13:deb1e793f125 111 return 0;
dkp14 0:74a5723d604a 112 }