ESC routines
ESC.cpp@0:c995af692179, 2016-05-30 (annotated)
- Committer:
- Decimus
- Date:
- Mon May 30 08:08:52 2016 +0000
- Revision:
- 0:c995af692179
[+]
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Decimus | 0:c995af692179 | 1 | /* mbed ESC Library |
Decimus | 0:c995af692179 | 2 | * Copyright (c) 2012 Oleg Evsegneev |
Decimus | 0:c995af692179 | 3 | * Released under the MIT License: http://mbed.org/license/mit |
Decimus | 0:c995af692179 | 4 | */ |
Decimus | 0:c995af692179 | 5 | |
Decimus | 0:c995af692179 | 6 | #include "ESC.h" |
Decimus | 0:c995af692179 | 7 | #include "mbed.h" |
Decimus | 0:c995af692179 | 8 | |
Decimus | 0:c995af692179 | 9 | //#define PPM_T 0.0025 |
Decimus | 0:c995af692179 | 10 | #define PPM_T 0.02 |
Decimus | 0:c995af692179 | 11 | |
Decimus | 0:c995af692179 | 12 | ESC::ESC(PinName x1, PinName x2, PinName y1, PinName y2, PinName led): |
Decimus | 0:c995af692179 | 13 | _esc_x1(x1), _esc_x2(x2), |
Decimus | 0:c995af692179 | 14 | _esc_y1(y1), _esc_y2(y2), |
Decimus | 0:c995af692179 | 15 | _led(led) { |
Decimus | 0:c995af692179 | 16 | |
Decimus | 0:c995af692179 | 17 | _esca[0] = &_esc_x1; |
Decimus | 0:c995af692179 | 18 | _esca[1] = &_esc_x2; |
Decimus | 0:c995af692179 | 19 | _esca[2] = &_esc_y1; |
Decimus | 0:c995af692179 | 20 | _esca[3] = &_esc_y2; |
Decimus | 0:c995af692179 | 21 | } |
Decimus | 0:c995af692179 | 22 | |
Decimus | 0:c995af692179 | 23 | void ESC::set( float x1, float x2, float y1, float y2 ){ |
Decimus | 0:c995af692179 | 24 | _esca[0]->write(x1); |
Decimus | 0:c995af692179 | 25 | _esca[1]->write(x2); |
Decimus | 0:c995af692179 | 26 | _esca[2]->write(y1); |
Decimus | 0:c995af692179 | 27 | _esca[3]->write(y2); |
Decimus | 0:c995af692179 | 28 | } |
Decimus | 0:c995af692179 | 29 | |
Decimus | 0:c995af692179 | 30 | void ESC::stop(){ |
Decimus | 0:c995af692179 | 31 | set(0.0, 0.0, 0.0, 0.0); |
Decimus | 0:c995af692179 | 32 | } |
Decimus | 0:c995af692179 | 33 | |
Decimus | 0:c995af692179 | 34 | void ESC::initialize_(){ |
Decimus | 0:c995af692179 | 35 | int i; |
Decimus | 0:c995af692179 | 36 | |
Decimus | 0:c995af692179 | 37 | #ifdef MODE_PPM |
Decimus | 0:c995af692179 | 38 | for( i=0; i<4; i++ ){ |
Decimus | 0:c995af692179 | 39 | // Calibrate Servo signal |
Decimus | 0:c995af692179 | 40 | _esca[i]->calibrate(0.0005, 90.0, PPM_T); |
Decimus | 0:c995af692179 | 41 | _esca[i]->write(0.0); |
Decimus | 0:c995af692179 | 42 | } |
Decimus | 0:c995af692179 | 43 | #else |
Decimus | 0:c995af692179 | 44 | for( i=0; i<4; i++ ){ |
Decimus | 0:c995af692179 | 45 | _esca[i]->period_us(250); |
Decimus | 0:c995af692179 | 46 | } |
Decimus | 0:c995af692179 | 47 | set(0.0, 0.0, 0.0, 0.0); |
Decimus | 0:c995af692179 | 48 | #endif |
Decimus | 0:c995af692179 | 49 | |
Decimus | 0:c995af692179 | 50 | #ifdef MODE_PWM |
Decimus | 0:c995af692179 | 51 | blink(2000); |
Decimus | 0:c995af692179 | 52 | set(0.2, 0.2, 0.2, 0.2); |
Decimus | 0:c995af692179 | 53 | blink(2000); |
Decimus | 0:c995af692179 | 54 | set(0.0, 0.0, 0.0, 0.0); |
Decimus | 0:c995af692179 | 55 | #endif |
Decimus | 0:c995af692179 | 56 | |
Decimus | 0:c995af692179 | 57 | blink(2000); |
Decimus | 0:c995af692179 | 58 | |
Decimus | 0:c995af692179 | 59 | |
Decimus | 0:c995af692179 | 60 | #ifdef CIRC_TEST |
Decimus | 0:c995af692179 | 61 | for( i=0; i<4; i++ ){ |
Decimus | 0:c995af692179 | 62 | _esca[i]->write(0.2); |
Decimus | 0:c995af692179 | 63 | _led = 1; |
Decimus | 0:c995af692179 | 64 | wait(3); |
Decimus | 0:c995af692179 | 65 | _esca[i]->write(0.0); |
Decimus | 0:c995af692179 | 66 | _led = 0; |
Decimus | 0:c995af692179 | 67 | wait(3); |
Decimus | 0:c995af692179 | 68 | } |
Decimus | 0:c995af692179 | 69 | #endif |
Decimus | 0:c995af692179 | 70 | } |
Decimus | 0:c995af692179 | 71 | |
Decimus | 0:c995af692179 | 72 | void ESC::calibrate_(){ |
Decimus | 0:c995af692179 | 73 | char i; |
Decimus | 0:c995af692179 | 74 | |
Decimus | 0:c995af692179 | 75 | // Calibrate PWM and set max throttle |
Decimus | 0:c995af692179 | 76 | for( i=0; i<4; i++ ){ |
Decimus | 0:c995af692179 | 77 | #ifdef MODE_PPM |
Decimus | 0:c995af692179 | 78 | _esca[i]->calibrate(0.0005, 90.0, PPM_T); |
Decimus | 0:c995af692179 | 79 | #endif |
Decimus | 0:c995af692179 | 80 | _esca[i]->write(1.0); |
Decimus | 0:c995af692179 | 81 | } |
Decimus | 0:c995af692179 | 82 | |
Decimus | 0:c995af692179 | 83 | // wait 5 seconds for battery |
Decimus | 0:c995af692179 | 84 | for( i=0; i<50; i++ ){ |
Decimus | 0:c995af692179 | 85 | wait(0.1); |
Decimus | 0:c995af692179 | 86 | _led = !_led; |
Decimus | 0:c995af692179 | 87 | } |
Decimus | 0:c995af692179 | 88 | |
Decimus | 0:c995af692179 | 89 | // wait 2 seconds to confirm max throttle |
Decimus | 0:c995af692179 | 90 | _led = 1; |
Decimus | 0:c995af692179 | 91 | wait(2); |
Decimus | 0:c995af692179 | 92 | _led = 0; |
Decimus | 0:c995af692179 | 93 | |
Decimus | 0:c995af692179 | 94 | // set min throttle |
Decimus | 0:c995af692179 | 95 | for( i=0; i<4; i++ ){ |
Decimus | 0:c995af692179 | 96 | _esca[i]->write(0.0); |
Decimus | 0:c995af692179 | 97 | } |
Decimus | 0:c995af692179 | 98 | } |
Decimus | 0:c995af692179 | 99 | |
Decimus | 0:c995af692179 | 100 | void ESC::blink(uint16_t delay){ |
Decimus | 0:c995af692179 | 101 | for(char i=0; i<delay/100; i++) { |
Decimus | 0:c995af692179 | 102 | _led = 1; |
Decimus | 0:c995af692179 | 103 | wait_ms(50); |
Decimus | 0:c995af692179 | 104 | _led = 0; |
Decimus | 0:c995af692179 | 105 | wait_ms(50); |
Decimus | 0:c995af692179 | 106 | } |
Decimus | 0:c995af692179 | 107 | } |