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

Committer:
NKarandey
Date:
Fri Mar 17 22:27:14 2017 +0000
Revision:
41:d52445129908
Parent:
40:04a05851ae7b
Child:
42:2ee563cd6223
Restore overwritten main

Who changed what in which revision?

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