Diff: com_func.cpp

Revision:

10:a2bd7d07c7f8

Child:

11:ff06edc0219c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/com_func.cpp	Wed Feb 10 14:58:50 2016 +0000
@@ -0,0 +1,149 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "TextLCD.h"
+#include "common.h"
+#include "com_func.h"
+
+
+void cf_led_onoff(
+    DigitalOut *led1,
+    DigitalOut *led2,
+    DigitalOut *led3,
+    DigitalOut *led4,
+    bool led1_onoff,
+    bool led2_onoff,
+    bool led3_onoff,
+    bool led4_onoff
+){
+    if (led1_onoff == true ) *led1 = 1;
+    else *led1 = 0;
+    if (led2_onoff == true ) *led2 = 1;
+    else *led2 = 0;
+    if (led3_onoff == true ) *led3 = 1;
+    else *led3 = 0;
+    if (led4_onoff == true ) *led4 = 1;
+    else *led4 = 0;
+}
+   
+// LED demo
+void cf_led_demo(
+    DigitalOut *led1,
+    DigitalOut *led2,
+    DigitalOut *led3,
+    DigitalOut *led4,
+    int cnt, 
+    int wait
+){
+    for( int i = 0; i < cnt; i++ ) {
+        *led1 = 1;   // on
+        *led2 = 0;   // off
+        *led3 = 0;   // off
+        *led4 = 0;   // off
+        Thread::wait(wait);
+        *led1 = 0;   // off
+        *led2 = 1;   // on
+        *led3 = 0;   // off
+        *led4 = 0;   // off
+        Thread::wait(wait);
+        *led1 = 0;   // off
+        *led2 = 0;   // off
+        *led3 = 1;   // on
+        *led4 = 0;   // off
+        Thread::wait(wait);
+        *led1 = 0;   // off
+        *led2 = 0;   // off
+        *led3 = 0;   // off
+        *led4 = 1;   // on
+        Thread::wait(wait);
+        *led1 = 0;   // off
+        *led2 = 0;   // off
+        *led3 = 0;   // off
+        *led4 = 0;   // off
+    }
+}
+
+void cf_led_error(
+    DigitalOut *led1,
+    DigitalOut *led2,
+    DigitalOut *led3,
+    DigitalOut *led4    
+){
+    for( int i = 0; i < 16; i++ ) {
+        *led1 = 1;   // on
+        *led2 = 1;   // off
+        *led3 = 1;   // off
+        *led4 = 1;   // off
+        Thread::wait(30);
+        *led1 = 0;   // off
+        *led2 = 0;   // on
+        *led3 = 0;   // off
+        *led4 = 0;   // off
+        Thread::wait(30);
+    }
+}
+
+void dspSetValue2Console(Serial* pc, setValue_t * setValue){
+#ifdef __DEBUG__    
+    pc->printf("-- JS Operation mode ----\n");
+    if( setValue->operation.sv_JS_OpeMode == 0 ){
+        pc->printf("CrExp Shape: I\n");
+    }
+    else{
+        pc->printf("CrExp Shape: KO\n");
+    }
+    if( setValue->operation.sv_JS_OpeMode == 0 ){
+        pc->printf("Single JoyStick mode\n");
+    }
+    else{
+        pc->printf("Dual JoyStick mode\n");
+    }
+    pc->printf("-- Transform motor ----\n");
+    pc->printf("RF FWD Thshold: 0x%04x(%d)\n", setValue->tfmCtrl.sv_RFTM_ith_F, setValue->tfmCtrl.sv_RFTM_ith_F);
+    pc->printf("RF RVS Thshold: 0x%04x(%d)\n", setValue->tfmCtrl.sv_RFTM_ith_R, setValue->tfmCtrl.sv_RFTM_ith_R);
+    pc->printf("LB FWD Thshold: 0x%04x(%d)\n", setValue->tfmCtrl.sv_LBTM_ith_F, setValue->tfmCtrl.sv_LBTM_ith_F);
+    pc->printf("LB RVS Thshold: 0x%04x(%d)\n", setValue->tfmCtrl.sv_LBTM_ith_R, setValue->tfmCtrl.sv_LBTM_ith_R);
+
+    pc->printf("RF FWD SpeedLt: 0x%02x(%d)\n", setValue->tfmCtrl.sv_RFTM_sli_F, setValue->tfmCtrl.sv_RFTM_sli_F);                
+    pc->printf("RF RVS SpeedLt: 0x%02x(%d)\n", setValue->tfmCtrl.sv_RFTM_sli_R, setValue->tfmCtrl.sv_RFTM_sli_R);                
+    pc->printf("RF M-Max Speed: 0x%02x(%d)\n", setValue->tfmCtrl.sv_RFTM_mst, setValue->tfmCtrl.sv_RFTM_mst);                
+    pc->printf("LB FWD SpeedLt: 0x%02x(%d)\n", setValue->tfmCtrl.sv_LBTM_sli_F, setValue->tfmCtrl.sv_LBTM_sli_F);                
+    pc->printf("LB RVS SpeedLt: 0x%02x(%d)\n", setValue->tfmCtrl.sv_LBTM_sli_R, setValue->tfmCtrl.sv_LBTM_sli_R);                
+    pc->printf("LB M-Max Speed: 0x%02x(%d)\n", setValue->tfmCtrl.sv_LBTM_mst, setValue->tfmCtrl.sv_LBTM_mst);                
+
+    pc->printf("-- Winch motor ----\n");
+    pc->printf("WD FWD Thshold: 0x%04x(%d)\n", setValue->winchCtrl.sv_WDM_ith_F, setValue->winchCtrl.sv_WDM_ith_F);
+    pc->printf("WD RVS Thshold: 0x%04x(%d)\n", setValue->winchCtrl.sv_WDM_ith_R, setValue->winchCtrl.sv_WDM_ith_R);
+    pc->printf("WR FWD Thshold: 0x%04x(%d)\n", setValue->winchCtrl.sv_WRM_ith_F, setValue->winchCtrl.sv_WRM_ith_F);
+    pc->printf("WR RVS Thshold: 0x%04x(%d)\n", setValue->winchCtrl.sv_WRM_ith_R, setValue->winchCtrl.sv_WRM_ith_R);     
+
+    pc->printf("WD FWD SpeedLt: 0x%02x(%d)\n", setValue->winchCtrl.sv_WDM_sli_F, setValue->winchCtrl.sv_WDM_sli_F);                
+    pc->printf("WD RVS SpeedLt: 0x%02x(%d)\n", setValue->winchCtrl.sv_WDM_sli_R, setValue->winchCtrl.sv_WDM_sli_R);                
+    pc->printf("WD M-Max Speed: 0x%02x(%d)\n", setValue->winchCtrl.sv_WDM_mst, setValue->winchCtrl.sv_WDM_mst);                
+    pc->printf("WD M-Max Speed: 0x%02x(%d)\n", setValue->winchCtrl.sv_WDM_mrt, setValue->winchCtrl.sv_WDM_mrt);                
+    pc->printf("WR FWD SpeedLt: 0x%02x(%d)\n", setValue->winchCtrl.sv_WRM_sli_F, setValue->winchCtrl.sv_WRM_sli_F);                
+    pc->printf("WR RVS SpeedLt: 0x%02x(%d)\n", setValue->winchCtrl.sv_WRM_sli_R, setValue->winchCtrl.sv_WRM_sli_R);                
+    pc->printf("WR FWD LowSpeedLt: 0x%02x(%d)\n", setValue->winchCtrl.sv_WRM_mst, setValue->winchCtrl.sv_WRM_mst);  
+    pc->printf("WR RVS LowSpeedLt: 0x%02x(%d)\n", setValue->winchCtrl.sv_WRM_mrt, setValue->winchCtrl.sv_WRM_mrt);  
+
+    pc->printf("-- Crawler motor ----\n");
+    pc->printf("RC FWD Thshold: 0x%04x(%d)\n", setValue->crawlerCtrl.sv_RFCM_ith_F, setValue->crawlerCtrl.sv_RFCM_ith_F);
+    pc->printf("RC RVS Thshold: 0x%04x(%d)\n", setValue->crawlerCtrl.sv_RFCM_ith_R, setValue->crawlerCtrl.sv_RFCM_ith_R);
+    pc->printf("LC FWD Thshold: 0x%04x(%d)\n", setValue->crawlerCtrl.sv_LBCM_ith_F, setValue->crawlerCtrl.sv_LBCM_ith_F);
+    pc->printf("LC RVS Thshold: 0x%04x(%d)\n", setValue->crawlerCtrl.sv_LBCM_ith_R, setValue->crawlerCtrl.sv_LBCM_ith_R);
+
+    pc->printf("RC FWD SpeedLt: 0x%02x(%d)\n", setValue->crawlerCtrl.sv_RFCM_sli_F, setValue->crawlerCtrl.sv_RFCM_sli_F);
+    pc->printf("RC RVS SpeedLt: 0x%02x(%d)\n", setValue->crawlerCtrl.sv_RFCM_sli_R, setValue->crawlerCtrl.sv_RFCM_sli_R);                
+    pc->printf("RC M-Max Speed: 0x%02x(%d)\n", setValue->crawlerCtrl.sv_RFCM_mst, setValue->crawlerCtrl.sv_RFCM_mst);
+    pc->printf("RC DeadZone U : 0x%02x(%d)\n", setValue->crawlerCtrl.sv_RFCM_dzu, setValue->crawlerCtrl.sv_RFCM_dzu);
+    pc->printf("RC DeadZone C : 0x%02x(%d)\n", setValue->crawlerCtrl.sv_RFCM_dzc, setValue->crawlerCtrl.sv_RFCM_dzc);
+    pc->printf("RC DeadZone L : 0x%02x(%d)\n", setValue->crawlerCtrl.sv_RFCM_dzl, setValue->crawlerCtrl.sv_RFCM_dzl);
+    pc->printf("LC FWD SpeedLt: 0x%02x(%d)\n", setValue->crawlerCtrl.sv_LBCM_sli_F, setValue->crawlerCtrl.sv_LBCM_sli_F);                
+    pc->printf("LC RVS SpeedLt: 0x%02x(%d)\n", setValue->crawlerCtrl.sv_LBCM_sli_R, setValue->crawlerCtrl.sv_LBCM_sli_R);                
+    pc->printf("LC M-Max Speed: 0x%02x(%d)\n", setValue->crawlerCtrl.sv_LBCM_mst, setValue->crawlerCtrl.sv_LBCM_mst);              
+    pc->printf("LC DeadZone U : 0x%02x(%d)\n", setValue->crawlerCtrl.sv_LBCM_dzu, setValue->crawlerCtrl.sv_LBCM_dzu);              
+    pc->printf("LC DeadZone C : 0x%02x(%d)\n", setValue->crawlerCtrl.sv_LBCM_dzc, setValue->crawlerCtrl.sv_LBCM_dzc);              
+    pc->printf("LC DeadZone L : 0x%02x(%d)\n", setValue->crawlerCtrl.sv_LBCM_dzl, setValue->crawlerCtrl.sv_LBCM_dzl);              
+    pc->printf( "\n" );
+#endif   
+}
+