Diff: ESC.cpp

Revision:

0:c995af692179

diff -r 000000000000 -r c995af692179 ESC.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ESC.cpp	Mon May 30 08:08:52 2016 +0000
@@ -0,0 +1,107 @@
+/* mbed ESC Library
+ * Copyright (c) 2012 Oleg Evsegneev
+ * Released under the MIT License: http://mbed.org/license/mit
+ */
+ 
+#include "ESC.h"
+#include "mbed.h"
+
+//#define PPM_T 0.0025
+#define PPM_T 0.02
+
+ESC::ESC(PinName x1, PinName x2, PinName y1, PinName y2, PinName led): 
+    _esc_x1(x1), _esc_x2(x2),
+    _esc_y1(y1), _esc_y2(y2),
+    _led(led) {
+    
+    _esca[0] = &_esc_x1;
+    _esca[1] = &_esc_x2;
+    _esca[2] = &_esc_y1;
+    _esca[3] = &_esc_y2;    
+}
+    
+void ESC::set( float x1, float x2, float y1, float y2 ){
+    _esca[0]->write(x1);
+    _esca[1]->write(x2);
+    _esca[2]->write(y1);
+    _esca[3]->write(y2);
+}
+
+void ESC::stop(){
+    set(0.0, 0.0, 0.0, 0.0);
+}
+
+void ESC::initialize_(){
+    int i;
+    
+    #ifdef MODE_PPM
+    for( i=0; i<4; i++ ){
+        // Calibrate Servo signal
+        _esca[i]->calibrate(0.0005, 90.0, PPM_T);
+        _esca[i]->write(0.0);
+    }
+    #else
+    for( i=0; i<4; i++ ){
+        _esca[i]->period_us(250);
+    }
+    set(0.0, 0.0, 0.0, 0.0);
+    #endif
+
+    #ifdef MODE_PWM
+    blink(2000);
+    set(0.2, 0.2, 0.2, 0.2);
+    blink(2000);
+    set(0.0, 0.0, 0.0, 0.0);
+    #endif
+
+    blink(2000);
+
+   
+    #ifdef CIRC_TEST
+    for( i=0; i<4; i++ ){
+        _esca[i]->write(0.2);
+        _led = 1;
+        wait(3);
+        _esca[i]->write(0.0);
+        _led = 0;
+        wait(3);
+    }
+    #endif
+}
+
+void ESC::calibrate_(){
+    char i;
+    
+    // Calibrate PWM and set max throttle
+    for( i=0; i<4; i++ ){
+        #ifdef MODE_PPM
+        _esca[i]->calibrate(0.0005, 90.0, PPM_T);
+        #endif
+        _esca[i]->write(1.0);
+    }
+
+    // wait 5 seconds for battery    
+    for( i=0; i<50; i++ ){
+        wait(0.1);
+        _led = !_led;
+    }
+    
+    // wait 2 seconds to confirm max throttle
+    _led = 1;
+    wait(2);
+    _led = 0;
+
+    // set min throttle
+    for( i=0; i<4; i++ ){
+        _esca[i]->write(0.0);
+    }
+}
+
+void ESC::blink(uint16_t delay){
+    for(char i=0; i<delay/100; i++) {
+        _led = 1;
+        wait_ms(50);                     
+        _led = 0; 
+        wait_ms(50);
+    }
+}
\ No newline at end of file