ESC.cpp

00001 /* mbed ESC Library
00002  * Copyright (c) 2012 Oleg Evsegneev
00003  * Released under the MIT License: http://mbed.org/license/mit
00004  */
00005  
00006 #include "ESC.h"
00007 #include "mbed.h"
00008 
00009 //#define PPM_T 0.0025
00010 #define PPM_T 0.02
00011 
00012 ESC::ESC(PinName x1, PinName x2, PinName y1, PinName y2, PinName led): 
00013     _esc_x1(x1), _esc_x2(x2),
00014     _esc_y1(y1), _esc_y2(y2),
00015     _led(led) {
00016     
00017     _esca[0] = &_esc_x1;
00018     _esca[1] = &_esc_x2;
00019     _esca[2] = &_esc_y1;
00020     _esca[3] = &_esc_y2;    
00021 }
00022     
00023 void ESC::set( float x1, float x2, float y1, float y2 ){
00024     _esca[0]->write(x1);
00025     _esca[1]->write(x2);
00026     _esca[2]->write(y1);
00027     _esca[3]->write(y2);
00028 }
00029 
00030 void ESC::stop(){
00031     set(0.0, 0.0, 0.0, 0.0);
00032 }
00033 
00034 void ESC::initialize_(){
00035     int i;
00036     
00037     #ifdef MODE_PPM
00038     for( i=0; i<4; i++ ){
00039         // Calibrate Servo signal
00040         _esca[i]->calibrate(0.0005, 90.0, PPM_T);
00041         _esca[i]->write(0.0);
00042     }
00043     #else
00044     for( i=0; i<4; i++ ){
00045         _esca[i]->period_us(250);
00046     }
00047     set(0.0, 0.0, 0.0, 0.0);
00048     #endif
00049 
00050     #ifdef MODE_PWM
00051     blink(2000);
00052     set(0.2, 0.2, 0.2, 0.2);
00053     blink(2000);
00054     set(0.0, 0.0, 0.0, 0.0);
00055     #endif
00056 
00057     blink(2000);
00058 
00059    
00060     #ifdef CIRC_TEST
00061     for( i=0; i<4; i++ ){
00062         _esca[i]->write(0.2);
00063         _led = 1;
00064         wait(3);
00065         _esca[i]->write(0.0);
00066         _led = 0;
00067         wait(3);
00068     }
00069     #endif
00070 }
00071 
00072 void ESC::calibrate_(){
00073     char i;
00074     
00075     // Calibrate PWM and set max throttle
00076     for( i=0; i<4; i++ ){
00077         #ifdef MODE_PPM
00078         _esca[i]->calibrate(0.0005, 90.0, PPM_T);
00079         #endif
00080         _esca[i]->write(1.0);
00081     }
00082 
00083     // wait 5 seconds for battery    
00084     for( i=0; i<50; i++ ){
00085         wait(0.1);
00086         _led = !_led;
00087     }
00088     
00089     // wait 2 seconds to confirm max throttle
00090     _led = 1;
00091     wait(2);
00092     _led = 0;
00093 
00094     // set min throttle
00095     for( i=0; i<4; i++ ){
00096         _esca[i]->write(0.0);
00097     }
00098 }
00099 
00100 void ESC::blink(uint16_t delay){
00101     for(char i=0; i<delay/100; i++) {
00102         _led = 1;
00103         wait_ms(50);                     
00104         _led = 0; 
00105         wait_ms(50);
00106     }
00107 }