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@92:52a91656458a, 2019-05-13 (annotated)

Committer:
joel_ssc
Date:
Mon May 13 19:25:26 2019 +0000
Revision:
92:52a91656458a
Parent:
74:d281aaef9766 
version for first flight test, timeouts not yet set correctly

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 74:d281aaef9766 19 _center_pwm = 1640.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 74:d281aaef9766 26
tnhnrl 74:d281aaef9766 27 _paused = false;
tnhnrl 52:f207567d3ea4 28 }
tnhnrl 52:f207567d3ea4 29
tnhnrl 52:f207567d3ea4 30 /////////////////////////////////////////////////////////////////////////////
tnhnrl 52:f207567d3ea4 31 // PWM TIMING SETUP AND SUPPORT FUNCTIONS
tnhnrl 52:f207567d3ea4 32
tnhnrl 52:f207567d3ea4 33 // initialize the ticker that will pulse on, start an off ticker, off-ticker pulses off and detaches, rinse and repeat
tnhnrl 52:f207567d3ea4 34 void ServoDriver::init() {
tnhnrl 52:f207567d3ea4 35 }
tnhnrl 52:f207567d3ea4 36
tnhnrl 52:f207567d3ea4 37 void ServoDriver::runServo() {
tnhnrl 52:f207567d3ea4 38 pwm_pulse_on();
tnhnrl 52:f207567d3ea4 39 }
tnhnrl 52:f207567d3ea4 40
tnhnrl 52:f207567d3ea4 41 void ServoDriver::pwm_pulse_off() {
tnhnrl 52:f207567d3ea4 42 _pwm_out = 0; //changed the pulse on and off to TIMEOUT so you don't have to detach anything
tnhnrl 52:f207567d3ea4 43 }
tnhnrl 52:f207567d3ea4 44
tnhnrl 52:f207567d3ea4 45 void ServoDriver::pwm_pulse_on() {
tnhnrl 74:d281aaef9766 46 if (_paused) {
tnhnrl 74:d281aaef9766 47 pwm_pulse_off_timeout.detach();
tnhnrl 74:d281aaef9766 48 }
tnhnrl 74:d281aaef9766 49 else {
tnhnrl 74:d281aaef9766 50 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 74:d281aaef9766 51 }
tnhnrl 52:f207567d3ea4 52 _pwm_out = 1;
tnhnrl 52:f207567d3ea4 53 }
tnhnrl 52:f207567d3ea4 54
tnhnrl 52:f207567d3ea4 55 void ServoDriver::pause() {
tnhnrl 74:d281aaef9766 56 //pwm_pulse_on_ticker.detach(); //run a pwm pulse on a digital out pin that has a period of 20 ms
tnhnrl 52:f207567d3ea4 57 }
tnhnrl 52:f207567d3ea4 58
tnhnrl 52:f207567d3ea4 59 void ServoDriver::unpause() {
tnhnrl 74:d281aaef9766 60 //init();
tnhnrl 52:f207567d3ea4 61 }
tnhnrl 52:f207567d3ea4 62
tnhnrl 52:f207567d3ea4 63 // SERVO OUTPUT MAPPING
tnhnrl 52:f207567d3ea4 64
tnhnrl 52:f207567d3ea4 65 // this function converts a desired degree location into a PWM command
tnhnrl 52:f207567d3ea4 66 void ServoDriver::setPosition_deg(float input_deg) {
tnhnrl 52:f207567d3ea4 67 // make sure desired angle is within the min and max deflection angles
tnhnrl 52:f207567d3ea4 68 _degrees_set_position = servoClamp<float>(input_deg, _min_deg, _max_deg);
tnhnrl 52:f207567d3ea4 69
tnhnrl 52:f207567d3ea4 70 // servo calibration
tnhnrl 52:f207567d3ea4 71 // slope is pwm signal over degrees (y = mx + b) using linear fit
tnhnrl 52:f207567d3ea4 72 // different slopes for above or below center!
tnhnrl 52:f207567d3ea4 73 float slope;
tnhnrl 52:f207567d3ea4 74 if (_degrees_set_position > 0)
tnhnrl 52:f207567d3ea4 75 slope = (_center_pwm - _min_pwm) / (-_min_deg);
tnhnrl 52:f207567d3ea4 76
tnhnrl 52:f207567d3ea4 77 else if (_degrees_set_position < 0)
tnhnrl 52:f207567d3ea4 78 slope = (_max_pwm - _center_pwm) / (_max_deg);
tnhnrl 52:f207567d3ea4 79
tnhnrl 52:f207567d3ea4 80 else
tnhnrl 52:f207567d3ea4 81 slope = 0.0;
tnhnrl 52:f207567d3ea4 82
tnhnrl 52:f207567d3ea4 83 // calculate linear servo response (output = slope*input + offset)
tnhnrl 52:f207567d3ea4 84 // negative slope needed for correct direction response
tnhnrl 52:f207567d3ea4 85 float initial_output = (slope * _degrees_set_position) + _center_pwm;
tnhnrl 52:f207567d3ea4 86
tnhnrl 52:f207567d3ea4 87 // saturate the pwm output to the min/max pwm limits
tnhnrl 52:f207567d3ea4 88 // remember the min/max pwm values may not be (min < max)
tnhnrl 74:d281aaef9766 89 // Basically handles the direction of movement
tnhnrl 52:f207567d3ea4 90 if (_max_pwm > _min_pwm)
tnhnrl 52:f207567d3ea4 91 _valid_servo_position_pwm = servoClamp<float>(initial_output, _min_pwm, _max_pwm);
tnhnrl 52:f207567d3ea4 92
tnhnrl 52:f207567d3ea4 93 else if (_max_pwm < _min_pwm)
tnhnrl 52:f207567d3ea4 94 _valid_servo_position_pwm = servoClamp<float>(initial_output, _max_pwm, _min_pwm);
tnhnrl 74:d281aaef9766 95 }
tnhnrl 74:d281aaef9766 96
tnhnrl 74:d281aaef9766 97 //the comparison below is used to swap the rotation of the servo
tnhnrl 74:d281aaef9766 98
tnhnrl 74:d281aaef9766 99 void ServoDriver::setPWM(float input_pwm) {
tnhnrl 74:d281aaef9766 100 if (_max_pwm > _min_pwm)
tnhnrl 74:d281aaef9766 101 _valid_servo_position_pwm = (unsigned int) servoClamp<float>(input_pwm, _min_pwm, _max_pwm); //volatile unsigned int
tnhnrl 52:f207567d3ea4 102
tnhnrl 74:d281aaef9766 103 else if (_min_pwm > _max_pwm)
tnhnrl 74:d281aaef9766 104 _valid_servo_position_pwm = (unsigned int) servoClamp<float>(input_pwm, _max_pwm, _min_pwm); //volatile unsigned int
tnhnrl 52:f207567d3ea4 105 }
tnhnrl 52:f207567d3ea4 106
tnhnrl 52:f207567d3ea4 107 ///////////////////////////////////////////////////////////////////////////
tnhnrl 52:f207567d3ea4 108 // SETTING PARAMETERS
tnhnrl 52:f207567d3ea4 109 void ServoDriver::setMinPWM(float pwm_input) {
tnhnrl 52:f207567d3ea4 110 _min_pwm = pwm_input;
tnhnrl 52:f207567d3ea4 111 }
tnhnrl 52:f207567d3ea4 112
tnhnrl 52:f207567d3ea4 113 void ServoDriver::setMaxPWM(float pwm_input) {
tnhnrl 52:f207567d3ea4 114 _max_pwm = pwm_input;
tnhnrl 52:f207567d3ea4 115 }
tnhnrl 52:f207567d3ea4 116
tnhnrl 52:f207567d3ea4 117 void ServoDriver::setCenterPWM(float pwm_input) {
tnhnrl 52:f207567d3ea4 118 _center_pwm = pwm_input;
tnhnrl 52:f207567d3ea4 119 }
tnhnrl 52:f207567d3ea4 120
tnhnrl 52:f207567d3ea4 121 void ServoDriver::setMinDeg(float deg) {
tnhnrl 52:f207567d3ea4 122 _min_deg = deg;
tnhnrl 52:f207567d3ea4 123 }
tnhnrl 52:f207567d3ea4 124
tnhnrl 52:f207567d3ea4 125 void ServoDriver::setMaxDeg(float deg) {
tnhnrl 52:f207567d3ea4 126 _max_deg = deg;
tnhnrl 52:f207567d3ea4 127 }
tnhnrl 52:f207567d3ea4 128
tnhnrl 52:f207567d3ea4 129 ///////////////////////////////////////////////////////////////////////////
tnhnrl 52:f207567d3ea4 130 // READING PARAMETERS
tnhnrl 74:d281aaef9766 131 float ServoDriver::getSetPosition_deg() { //rename to getSetPosition later for consistency
tnhnrl 52:f207567d3ea4 132 return _degrees_set_position;
tnhnrl 52:f207567d3ea4 133 }
tnhnrl 52:f207567d3ea4 134
tnhnrl 52:f207567d3ea4 135 // returns pwm signal in microseconds, for example: 1580 microseconds
tnhnrl 74:d281aaef9766 136 float ServoDriver::getSetPosition_pwm() {
tnhnrl 52:f207567d3ea4 137 return (float)_valid_servo_position_pwm; //servo is unsigned int
tnhnrl 52:f207567d3ea4 138 }
tnhnrl 52:f207567d3ea4 139
tnhnrl 52:f207567d3ea4 140 float ServoDriver::getMinPWM() {
tnhnrl 52:f207567d3ea4 141 return _min_pwm;
tnhnrl 52:f207567d3ea4 142 }
tnhnrl 52:f207567d3ea4 143
tnhnrl 52:f207567d3ea4 144 float ServoDriver::getMaxPWM() {
tnhnrl 52:f207567d3ea4 145 return _max_pwm;
tnhnrl 52:f207567d3ea4 146 }
tnhnrl 52:f207567d3ea4 147
tnhnrl 52:f207567d3ea4 148 float ServoDriver::getCenterPWM() {
tnhnrl 52:f207567d3ea4 149 return _center_pwm;
tnhnrl 52:f207567d3ea4 150 }
tnhnrl 52:f207567d3ea4 151
tnhnrl 52:f207567d3ea4 152 float ServoDriver::getMinDeg() {
tnhnrl 52:f207567d3ea4 153 return _min_deg;
tnhnrl 52:f207567d3ea4 154 }
tnhnrl 52:f207567d3ea4 155
tnhnrl 52:f207567d3ea4 156 float ServoDriver::getMaxDeg() {
tnhnrl 52:f207567d3ea4 157 return _max_deg;
tnhnrl 52:f207567d3ea4 158 }