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

photointerruptRoutines.h@50:c0c299bf54a6, 2017-03-24 (annotated)

Committer:
davidanderle
Date:
Fri Mar 24 17:30:41 2017 +0000
Revision:
50:c0c299bf54a6
Parent:
49:731c95cd5852
Child:
53:c703e0bf5e87
Child:
56:389436192e63
Velocity PID control tuned to an error of +-1 rev/sec

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NKarandey 49:731c95cd5852 1 volatile float w3 = 0; //Angular velocity
NKarandey 49:731c95cd5852 2 volatile int count_i3 = 0;
NKarandey 49:731c95cd5852 3
NKarandey 49:731c95cd5852 4 volatile int CHA_state = 0x00;
NKarandey 49:731c95cd5852 5 volatile int CHB_state = 0x00;
NKarandey 49:731c95cd5852 6 volatile int CH_state = 0x00;
NKarandey 49:731c95cd5852 7 volatile int CH_state_prev = 0x00;
NKarandey 49:731c95cd5852 8
NKarandey 49:731c95cd5852 9 volatile float diskPosition = 0.0; //in degrees
NKarandey 49:731c95cd5852 10
NKarandey 49:731c95cd5852 11 volatile float velocityDuty = 0.0;
NKarandey 49:731c95cd5852 12
NKarandey 49:731c95cd5852 13 Timer dt_I3;
NKarandey 49:731c95cd5852 14
NKarandey 49:731c95cd5852 15 int8_t updateState() { //Convert photointerrupter inputs to a
NKarandey 49:731c95cd5852 16 return stateMap[4*I3 + 2*I2 + I1]; // rotor state. Reports 1/6 positions
NKarandey 49:731c95cd5852 17 }
NKarandey 49:731c95cd5852 18
NKarandey 49:731c95cd5852 19 void updateDiskPosition() {
NKarandey 49:731c95cd5852 20 if (CH_state != CH_state_prev) {
NKarandey 49:731c95cd5852 21 int diff = CH_state - CH_state_prev;
NKarandey 49:731c95cd5852 22
NKarandey 49:731c95cd5852 23 CH_state_prev = CH_state;
NKarandey 49:731c95cd5852 24 if (abs(diff) == 1 || abs(diff) == 3) {
NKarandey 49:731c95cd5852 25 if (diff < 0)
NKarandey 49:731c95cd5852 26 diskPosition += angularResolution;
NKarandey 49:731c95cd5852 27 else
NKarandey 49:731c95cd5852 28 diskPosition -= angularResolution;
NKarandey 49:731c95cd5852 29 }
NKarandey 49:731c95cd5852 30 else if (abs(diff) == 2) {
NKarandey 49:731c95cd5852 31 if (diff < 0)
NKarandey 49:731c95cd5852 32 diskPosition += 2.0f * angularResolution;
NKarandey 49:731c95cd5852 33 else
NKarandey 49:731c95cd5852 34 diskPosition -= 2.0f * angularResolution;
NKarandey 49:731c95cd5852 35 }
NKarandey 49:731c95cd5852 36
NKarandey 49:731c95cd5852 37 if (diskPosition >= 360.0f) {
NKarandey 49:731c95cd5852 38 diskPosition -= 360.0f;
NKarandey 49:731c95cd5852 39 } else if (diskPosition < -360.0f) {
NKarandey 49:731c95cd5852 40 diskPosition += 360.0f;
NKarandey 49:731c95cd5852 41 }
NKarandey 49:731c95cd5852 42 }
NKarandey 49:731c95cd5852 43 }
NKarandey 49:731c95cd5852 44
NKarandey 49:731c95cd5852 45 inline void updateRelativeState() {
NKarandey 49:731c95cd5852 46 CH_state = relativeStateMap[CHB_state + 2*CHA_state];
NKarandey 49:731c95cd5852 47 }
NKarandey 49:731c95cd5852 48
NKarandey 49:731c95cd5852 49 inline void CHA_rise() {
NKarandey 49:731c95cd5852 50 CHA_state = 1;
NKarandey 49:731c95cd5852 51 updateRelativeState();
NKarandey 49:731c95cd5852 52 updateDiskPosition();
NKarandey 49:731c95cd5852 53 }
NKarandey 49:731c95cd5852 54
NKarandey 49:731c95cd5852 55 // Takes 5-6us
NKarandey 49:731c95cd5852 56 inline void CHA_fall() {
NKarandey 49:731c95cd5852 57 CHA_state = 0;
NKarandey 49:731c95cd5852 58 updateRelativeState();
NKarandey 49:731c95cd5852 59 updateDiskPosition();
NKarandey 49:731c95cd5852 60 }
NKarandey 49:731c95cd5852 61 inline void CHB_rise() {
NKarandey 49:731c95cd5852 62 CHB_state = 1;
NKarandey 49:731c95cd5852 63 updateRelativeState();
NKarandey 49:731c95cd5852 64 updateDiskPosition();
NKarandey 49:731c95cd5852 65 }
NKarandey 49:731c95cd5852 66 inline void CHB_fall() {
NKarandey 49:731c95cd5852 67 CHB_state = 0;
NKarandey 49:731c95cd5852 68 updateRelativeState();
NKarandey 49:731c95cd5852 69 updateDiskPosition();
NKarandey 49:731c95cd5852 70 }
NKarandey 49:731c95cd5852 71
NKarandey 49:731c95cd5852 72 void i_edge(){
NKarandey 49:731c95cd5852 73 state = updateState();
davidanderle 50:c0c299bf54a6 74 motorOut((state-orState+lead+6)%6, velocityDuty);
NKarandey 49:731c95cd5852 75 }
NKarandey 49:731c95cd5852 76
NKarandey 49:731c95cd5852 77 inline void i3rise(){
NKarandey 49:731c95cd5852 78 state = updateState();
davidanderle 50:c0c299bf54a6 79 motorOut((state-orState+lead+6)%6, velocityDuty);
davidanderle 50:c0c299bf54a6 80 w3 = 1/dt_I3.read(); //Calc angular velocity
NKarandey 49:731c95cd5852 81 dt_I3.reset();
NKarandey 49:731c95cd5852 82
NKarandey 49:731c95cd5852 83 if (I2.read() == 1) //Only count revolutions if the
NKarandey 49:731c95cd5852 84 count_i3++; // rotor spins forward
NKarandey 49:731c95cd5852 85 }