William Lange / Mbed 2 deprecated DistanceFirmware

Same as mallet... but distance

Dependencies:   EthernetInterface NetworkAPI mbed-rtos mbed

Fork of MalletFirmware by Impact-Echo

MotorControl.cpp@30:1da0bb9c31a6, 2014-12-09 (annotated)

Committer:
willange
Date:
Tue Dec 09 17:37:12 2014 +0000
Revision:
30:1da0bb9c31a6
Parent:
22:523e316cbe70 
a basic timer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
timmey9 22:523e316cbe70 1 #include "MotorControl.h"
timmey9 22:523e316cbe70 2 #include "mbed.h"
timmey9 22:523e316cbe70 3
timmey9 22:523e316cbe70 4 // constructor
timmey9 22:523e316cbe70 5 MotorControl::MotorControl(PinName forward, PinName backward, int period, int safetyPeriod) :
timmey9 22:523e316cbe70 6 _forward(forward),
timmey9 22:523e316cbe70 7 _backward(backward) {
timmey9 22:523e316cbe70 8
timmey9 22:523e316cbe70 9 _forward = 0; // turn motor off
timmey9 22:523e316cbe70 10 _backward = 0; // turn motor off
timmey9 22:523e316cbe70 11
timmey9 22:523e316cbe70 12 _period = period;
timmey9 22:523e316cbe70 13 _safetyPeriod = safetyPeriod;
timmey9 22:523e316cbe70 14 }
timmey9 22:523e316cbe70 15
timmey9 22:523e316cbe70 16 // forward at full speed
timmey9 22:523e316cbe70 17 void MotorControl::forward(int duration) {
timmey9 22:523e316cbe70 18 if(duration < _safetyPeriod) return;
timmey9 22:523e316cbe70 19 if(duration < CHECK_TIME) return;
timmey9 22:523e316cbe70 20
timmey9 22:523e316cbe70 21 _forward = 0;
timmey9 22:523e316cbe70 22 _backward = 0;
timmey9 22:523e316cbe70 23 wait_us(_safetyPeriod);
timmey9 22:523e316cbe70 24
timmey9 22:523e316cbe70 25 int total_cycles = duration / CHECK_TIME;
timmey9 22:523e316cbe70 26
timmey9 22:523e316cbe70 27 // turn on forward direction
timmey9 22:523e316cbe70 28 _forward = 1;
timmey9 22:523e316cbe70 29 for(int i = 0; i < total_cycles; i++)
timmey9 22:523e316cbe70 30 {
timmey9 22:523e316cbe70 31 if(_backward.read()) break; // periodically check that _backward has not turned on
timmey9 22:523e316cbe70 32 wait_us(CHECK_TIME);
timmey9 22:523e316cbe70 33 }
timmey9 22:523e316cbe70 34
timmey9 22:523e316cbe70 35 _forward = 0;
timmey9 22:523e316cbe70 36 _backward = 0;
timmey9 22:523e316cbe70 37 wait_us(_safetyPeriod);
timmey9 22:523e316cbe70 38 }
timmey9 22:523e316cbe70 39
timmey9 22:523e316cbe70 40 // backward at full speed
timmey9 22:523e316cbe70 41 void MotorControl::backward(int duration) {
timmey9 22:523e316cbe70 42 if(duration < _safetyPeriod) return;
timmey9 22:523e316cbe70 43 if(duration < CHECK_TIME) return;
timmey9 22:523e316cbe70 44
timmey9 22:523e316cbe70 45 _forward = 0;
timmey9 22:523e316cbe70 46 _backward = 0;
timmey9 22:523e316cbe70 47 wait_us(_safetyPeriod);
timmey9 22:523e316cbe70 48
timmey9 22:523e316cbe70 49 int total_cycles = duration / CHECK_TIME;
timmey9 22:523e316cbe70 50
timmey9 22:523e316cbe70 51 // turn on forward direction
timmey9 22:523e316cbe70 52 _backward = 1;
timmey9 22:523e316cbe70 53 for(int i = 0; i < total_cycles; i++)
timmey9 22:523e316cbe70 54 {
timmey9 22:523e316cbe70 55 if(_forward.read()) break; // periodically check that _forward has not turned on
timmey9 22:523e316cbe70 56 wait_us(CHECK_TIME);
timmey9 22:523e316cbe70 57 }
timmey9 22:523e316cbe70 58
timmey9 22:523e316cbe70 59 _forward = 0;
timmey9 22:523e316cbe70 60 _backward = 0;
timmey9 22:523e316cbe70 61 wait_us(_safetyPeriod);
timmey9 22:523e316cbe70 62 }
timmey9 22:523e316cbe70 63
timmey9 22:523e316cbe70 64 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
timmey9 22:523e316cbe70 65 *
timmey9 22:523e316cbe70 66 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
timmey9 22:523e316cbe70 67 void MotorControl::forward(float duty_cycle, uint32_t duration) {
timmey9 22:523e316cbe70 68 if(duty_cycle > 1) return;
timmey9 22:523e316cbe70 69 if(duration < _period) return;
timmey9 22:523e316cbe70 70
timmey9 22:523e316cbe70 71 // turn off motors and wait sufficient time to guarantee motors are off
timmey9 22:523e316cbe70 72 _forward = 0;
timmey9 22:523e316cbe70 73 _backward = 0;
timmey9 22:523e316cbe70 74 wait_us(_safetyPeriod);
timmey9 22:523e316cbe70 75
timmey9 22:523e316cbe70 76 __disable_irq(); // Disable Interrupts
timmey9 22:523e316cbe70 77
timmey9 22:523e316cbe70 78 // perform some calculations
timmey9 22:523e316cbe70 79 uint32_t total_cycles = duration / _period;
timmey9 22:523e316cbe70 80 uint32_t on_cycles = (uint32_t) _period * duty_cycle;
timmey9 22:523e316cbe70 81 uint32_t off_cycles = _period - on_cycles;
timmey9 22:523e316cbe70 82 uint32_t on_cycles_sub = on_cycles / CHECK_TIME;
timmey9 22:523e316cbe70 83
timmey9 22:523e316cbe70 84
timmey9 22:523e316cbe70 85 for(int i = 0; i < total_cycles; i++)
timmey9 22:523e316cbe70 86 {
timmey9 22:523e316cbe70 87 if(_backward.read()) return;
timmey9 22:523e316cbe70 88 _forward = 1;
timmey9 22:523e316cbe70 89
timmey9 22:523e316cbe70 90 for(int j = 0; j < on_cycles_sub; j++)
timmey9 22:523e316cbe70 91 {
timmey9 22:523e316cbe70 92 if(_backward.read()) break; // periodically check that _backward is off
timmey9 22:523e316cbe70 93 wait_us(CHECK_TIME);
timmey9 22:523e316cbe70 94 }
timmey9 22:523e316cbe70 95
timmey9 22:523e316cbe70 96 _forward = 0;
timmey9 22:523e316cbe70 97 _backward = 0;
timmey9 22:523e316cbe70 98 wait_us(off_cycles);
timmey9 22:523e316cbe70 99 }
timmey9 22:523e316cbe70 100
timmey9 22:523e316cbe70 101 __enable_irq(); // Enable Interrupts
timmey9 22:523e316cbe70 102 wait_us(_safetyPeriod);
timmey9 22:523e316cbe70 103 }
timmey9 22:523e316cbe70 104
timmey9 22:523e316cbe70 105
timmey9 22:523e316cbe70 106 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
timmey9 22:523e316cbe70 107
timmey9 22:523e316cbe70 108 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
timmey9 22:523e316cbe70 109 void MotorControl::backward(float duty_cycle, uint32_t duration) {
timmey9 22:523e316cbe70 110 if(duty_cycle > 1) return;
timmey9 22:523e316cbe70 111 if(duration < _period) return;
timmey9 22:523e316cbe70 112
timmey9 22:523e316cbe70 113 // turn off motors and wait sufficient time to guarantee motors are off
timmey9 22:523e316cbe70 114 _forward = 0;
timmey9 22:523e316cbe70 115 _backward = 0;
timmey9 22:523e316cbe70 116 wait_us(_safetyPeriod);
timmey9 22:523e316cbe70 117
timmey9 22:523e316cbe70 118 __disable_irq(); // Disable Interrupts
timmey9 22:523e316cbe70 119
timmey9 22:523e316cbe70 120 // perform some calculations
timmey9 22:523e316cbe70 121 uint32_t total_cycles = duration / _period;
timmey9 22:523e316cbe70 122 uint32_t on_cycles = (uint32_t)_period * duty_cycle;
timmey9 22:523e316cbe70 123 uint32_t off_cycles = _period - on_cycles;
timmey9 22:523e316cbe70 124 uint32_t on_cycles_sub = on_cycles / CHECK_TIME;
timmey9 22:523e316cbe70 125
timmey9 22:523e316cbe70 126
timmey9 22:523e316cbe70 127 for(int i = 0; i < total_cycles; i++)
timmey9 22:523e316cbe70 128 {
timmey9 22:523e316cbe70 129 if(_forward.read()) return;
timmey9 22:523e316cbe70 130 _backward = 1;
timmey9 22:523e316cbe70 131
timmey9 22:523e316cbe70 132 for(int j = 0; j < on_cycles_sub; j++)
timmey9 22:523e316cbe70 133 {
timmey9 22:523e316cbe70 134 if(_forward.read()) break; // periodically check that _forward is off
timmey9 22:523e316cbe70 135 wait_us(CHECK_TIME);
timmey9 22:523e316cbe70 136 }
timmey9 22:523e316cbe70 137
timmey9 22:523e316cbe70 138 _forward = 0;
timmey9 22:523e316cbe70 139 _backward = 0;
timmey9 22:523e316cbe70 140 wait_us(off_cycles);
timmey9 22:523e316cbe70 141 }
timmey9 22:523e316cbe70 142
timmey9 22:523e316cbe70 143 __enable_irq(); // Enable Interrupts
timmey9 22:523e316cbe70 144 wait_us(_safetyPeriod);
timmey9 22:523e316cbe70 145 }