UAVRO / ESC

ESC routines

Dependents:   Q2_Stabi

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?

UserRevisionLine numberNew 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 }