Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown

Dependencies:   mbed MODSERIAL FATFileSystem

ServoDriver_new/ServoDriver.cpp@73:f6f378311c8d, 2018-07-30 (annotated)

Committer:
tnhnrl
Date:
Mon Jul 30 16:48:48 2018 +0000
Revision:
73:f6f378311c8d
Parent:
52:f207567d3ea4
Child:
74:d281aaef9766
work in progress 7/30 12:48 pm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tnhnrl 52:f207567d3ea4 1 // modifications by Dan 2018-02-28
tnhnrl 52:f207567d3ea4 2 // - mostly formatting consistency for readability
tnhnrl 52:f207567d3ea4 3 // - changed ServoDriver initial _valid_servo_position to the _center_pwm value
tnhnrl 52:f207567d3ea4 4 // - fixed problems with servo mapping function.
tnhnrl 52:f207567d3ea4 5 // -- made min_pwm match to min_deg, even if min_pwm is greater than max_pwm
tnhnrl 52:f207567d3ea4 6 // -- made max_pwm match to max_deg, even if max_pwm is less than min_pwm
tnhnrl 52:f207567d3ea4 7 // -- now linear mapping between min pwm/deg, center_pwm/0deg, max pwm/deg.
tnhnrl 52:f207567d3ea4 8
tnhnrl 52:f207567d3ea4 9 #include "ServoDriver.hpp"
tnhnrl 52:f207567d3ea4 10
tnhnrl 52:f207567d3ea4 11 // INNER LOOP ServoDriver has settings, endpoints, direction, neutral point
tnhnrl 52:f207567d3ea4 12
tnhnrl 52:f207567d3ea4 13 //this driver does not use pwmOut because of the period timer conflict with the H-bridge motor controllers
tnhnrl 52:f207567d3ea4 14
tnhnrl 52:f207567d3ea4 15 // initialize default calibration/parameter values
tnhnrl 52:f207567d3ea4 16 // note: (min_deg < max_deg) must be true!
tnhnrl 52:f207567d3ea4 17 ServoDriver::ServoDriver(PinName digital_pin) : _pwm_out(digital_pin) {
tnhnrl 52:f207567d3ea4 18 _period_cnt = 0;
tnhnrl 52:f207567d3ea4 19 _center_pwm = 1500.0; // mechanical center trimmed to be 1.5ms (1500us)
tnhnrl 52:f207567d3ea4 20 _min_deg = -45.0; // lower deflection against end stop
tnhnrl 52:f207567d3ea4 21 _max_deg = 45.0; // upper deflection against end stop
tnhnrl 52:f207567d3ea4 22 _min_pwm = 2240.0; // pwm corresponding to _min_deg
tnhnrl 52:f207567d3ea4 23 _max_pwm = 1040.0; // pwm corresponding to _max_deg
tnhnrl 52:f207567d3ea4 24
tnhnrl 52:f207567d3ea4 25 _valid_servo_position_pwm = (unsigned int)_center_pwm; // servo position on boot-up
tnhnrl 52:f207567d3ea4 26 }
tnhnrl 52:f207567d3ea4 27
tnhnrl 52:f207567d3ea4 28 /////////////////////////////////////////////////////////////////////////////
tnhnrl 52:f207567d3ea4 29 // PWM TIMING SETUP AND SUPPORT FUNCTIONS
tnhnrl 52:f207567d3ea4 30
tnhnrl 52:f207567d3ea4 31 // initialize the ticker that will pulse on, start an off ticker, off-ticker pulses off and detaches, rinse and repeat
tnhnrl 52:f207567d3ea4 32 void ServoDriver::init() {
tnhnrl 52:f207567d3ea4 33 }
tnhnrl 52:f207567d3ea4 34
tnhnrl 52:f207567d3ea4 35 void ServoDriver::runServo() {
tnhnrl 52:f207567d3ea4 36 pwm_pulse_on();
tnhnrl 52:f207567d3ea4 37 }
tnhnrl 52:f207567d3ea4 38
tnhnrl 52:f207567d3ea4 39 void ServoDriver::pwm_pulse_off() {
tnhnrl 52:f207567d3ea4 40 _pwm_out = 0; //changed the pulse on and off to TIMEOUT so you don't have to detach anything
tnhnrl 52:f207567d3ea4 41 }
tnhnrl 52:f207567d3ea4 42
tnhnrl 52:f207567d3ea4 43 void ServoDriver::pwm_pulse_on() {
tnhnrl 52:f207567d3ea4 44 pwm_pulse_off_timeout.attach_us(callback(this, &ServoDriver::pwm_pulse_off), _valid_servo_position_pwm); // _valid_servo_position_pwm is the signal in microseconds, e.g. 1500
tnhnrl 52:f207567d3ea4 45 _pwm_out = 1;
tnhnrl 52:f207567d3ea4 46 }
tnhnrl 52:f207567d3ea4 47
tnhnrl 52:f207567d3ea4 48 void ServoDriver::pause() {
tnhnrl 52:f207567d3ea4 49 pwm_pulse_on_ticker.detach(); //run a pwm pulse on a digital out pin that has a period of 20 ms
tnhnrl 52:f207567d3ea4 50 }
tnhnrl 52:f207567d3ea4 51
tnhnrl 52:f207567d3ea4 52 void ServoDriver::unpause() {
tnhnrl 52:f207567d3ea4 53 init();
tnhnrl 52:f207567d3ea4 54 }
tnhnrl 52:f207567d3ea4 55
tnhnrl 52:f207567d3ea4 56 // SERVO OUTPUT MAPPING
tnhnrl 52:f207567d3ea4 57
tnhnrl 52:f207567d3ea4 58 // this function converts a desired degree location into a PWM command
tnhnrl 52:f207567d3ea4 59 void ServoDriver::setPosition_deg(float input_deg) {
tnhnrl 52:f207567d3ea4 60 // make sure desired angle is within the min and max deflection angles
tnhnrl 52:f207567d3ea4 61 _degrees_set_position = servoClamp<float>(input_deg, _min_deg, _max_deg);
tnhnrl 52:f207567d3ea4 62
tnhnrl 52:f207567d3ea4 63 #if(0)
tnhnrl 52:f207567d3ea4 64
tnhnrl 52:f207567d3ea4 65 // servo calibration
tnhnrl 52:f207567d3ea4 66 // slope is pwm signal over degrees (y = mx + b) using linear fit
tnhnrl 52:f207567d3ea4 67 // different slopes for above or below center!
tnhnrl 52:f207567d3ea4 68 float slope;
tnhnrl 52:f207567d3ea4 69 if (_degrees_set_position > 0)
tnhnrl 52:f207567d3ea4 70 slope = (_center_pwm - _min_pwm) / (-_min_deg);
tnhnrl 52:f207567d3ea4 71
tnhnrl 52:f207567d3ea4 72 else if (_degrees_set_position < 0)
tnhnrl 52:f207567d3ea4 73 slope = (_max_pwm - _center_pwm) / (_max_deg);
tnhnrl 52:f207567d3ea4 74
tnhnrl 52:f207567d3ea4 75 else
tnhnrl 52:f207567d3ea4 76 slope = 0.0;
tnhnrl 52:f207567d3ea4 77
tnhnrl 52:f207567d3ea4 78 // calculate linear servo response (output = slope*input + offset)
tnhnrl 52:f207567d3ea4 79 // negative slope needed for correct direction response
tnhnrl 52:f207567d3ea4 80 float initial_output = (slope * _degrees_set_position) + _center_pwm;
tnhnrl 52:f207567d3ea4 81
tnhnrl 52:f207567d3ea4 82 // saturate the pwm output to the min/max pwm limits
tnhnrl 52:f207567d3ea4 83 // remember the min/max pwm values may not be (min < max)
tnhnrl 52:f207567d3ea4 84 if (_max_pwm > _min_pwm)
tnhnrl 52:f207567d3ea4 85 _valid_servo_position_pwm = servoClamp<float>(initial_output, _min_pwm, _max_pwm);
tnhnrl 52:f207567d3ea4 86
tnhnrl 52:f207567d3ea4 87 else if (_max_pwm < _min_pwm)
tnhnrl 52:f207567d3ea4 88 _valid_servo_position_pwm = servoClamp<float>(initial_output, _max_pwm, _min_pwm);
tnhnrl 52:f207567d3ea4 89 #else
tnhnrl 52:f207567d3ea4 90 float slope, out;
tnhnrl 52:f207567d3ea4 91
tnhnrl 52:f207567d3ea4 92 slope = (_max_pwm - _min_pwm)/( _max_deg - _min_deg );
tnhnrl 52:f207567d3ea4 93 out = slope*(_degrees_set_position - _min_deg) + _min_pwm;
tnhnrl 52:f207567d3ea4 94 _valid_servo_position_pwm = (unsigned int)out;
tnhnrl 52:f207567d3ea4 95 #endif
tnhnrl 52:f207567d3ea4 96 }
tnhnrl 52:f207567d3ea4 97
tnhnrl 52:f207567d3ea4 98 ///////////////////////////////////////////////////////////////////////////
tnhnrl 52:f207567d3ea4 99 // SETTING PARAMETERS
tnhnrl 52:f207567d3ea4 100 void ServoDriver::setMinPWM(float pwm_input) {
tnhnrl 52:f207567d3ea4 101 _min_pwm = pwm_input;
tnhnrl 52:f207567d3ea4 102 }
tnhnrl 52:f207567d3ea4 103
tnhnrl 52:f207567d3ea4 104 void ServoDriver::setMaxPWM(float pwm_input) {
tnhnrl 52:f207567d3ea4 105 _max_pwm = pwm_input;
tnhnrl 52:f207567d3ea4 106 }
tnhnrl 52:f207567d3ea4 107
tnhnrl 52:f207567d3ea4 108 void ServoDriver::setCenterPWM(float pwm_input) {
tnhnrl 52:f207567d3ea4 109 _center_pwm = pwm_input;
tnhnrl 52:f207567d3ea4 110 }
tnhnrl 52:f207567d3ea4 111
tnhnrl 52:f207567d3ea4 112 void ServoDriver::setMinDeg(float deg) {
tnhnrl 52:f207567d3ea4 113 _min_deg = deg;
tnhnrl 52:f207567d3ea4 114 }
tnhnrl 52:f207567d3ea4 115
tnhnrl 52:f207567d3ea4 116 void ServoDriver::setMaxDeg(float deg) {
tnhnrl 52:f207567d3ea4 117 _max_deg = deg;
tnhnrl 52:f207567d3ea4 118 }
tnhnrl 52:f207567d3ea4 119
tnhnrl 52:f207567d3ea4 120 ///////////////////////////////////////////////////////////////////////////
tnhnrl 52:f207567d3ea4 121 // READING PARAMETERS
tnhnrl 73:f6f378311c8d 122 float ServoDriver::getPosition_deg() { //rename to getSetPosition later for consistency
tnhnrl 52:f207567d3ea4 123 return _degrees_set_position;
tnhnrl 52:f207567d3ea4 124 }
tnhnrl 52:f207567d3ea4 125
tnhnrl 52:f207567d3ea4 126 // returns pwm signal in microseconds, for example: 1580 microseconds
tnhnrl 52:f207567d3ea4 127 float ServoDriver::getPosition_pwm() {
tnhnrl 52:f207567d3ea4 128 return (float)_valid_servo_position_pwm; //servo is unsigned int
tnhnrl 52:f207567d3ea4 129 }
tnhnrl 52:f207567d3ea4 130
tnhnrl 52:f207567d3ea4 131 float ServoDriver::getMinPWM() {
tnhnrl 52:f207567d3ea4 132 return _min_pwm;
tnhnrl 52:f207567d3ea4 133 }
tnhnrl 52:f207567d3ea4 134
tnhnrl 52:f207567d3ea4 135 float ServoDriver::getMaxPWM() {
tnhnrl 52:f207567d3ea4 136 return _max_pwm;
tnhnrl 52:f207567d3ea4 137 }
tnhnrl 52:f207567d3ea4 138
tnhnrl 52:f207567d3ea4 139 float ServoDriver::getCenterPWM() {
tnhnrl 52:f207567d3ea4 140 return _center_pwm;
tnhnrl 52:f207567d3ea4 141 }
tnhnrl 52:f207567d3ea4 142
tnhnrl 52:f207567d3ea4 143 float ServoDriver::getMinDeg() {
tnhnrl 52:f207567d3ea4 144 return _min_deg;
tnhnrl 52:f207567d3ea4 145 }
tnhnrl 52:f207567d3ea4 146
tnhnrl 52:f207567d3ea4 147 float ServoDriver::getMaxDeg() {
tnhnrl 52:f207567d3ea4 148 return _max_deg;
tnhnrl 52:f207567d3ea4 149 }