z ysaito / Mbed 2 deprecated Pro_B2_2ZNR_HandyCtrler

2018.07.26

Dependencies:   TextLCD USBDevice mbed-rtos mbed

2_main.cpp@1:368ba89c2e6b, 2016-03-28 (annotated)

Committer:
sayzyas
Date:
Mon Mar 28 00:09:18 2016 +0000
Revision:
1:368ba89c2e6b
Parent:
0:f4e2a62331ef
Child:
2:d0a5ee4e7bb8
Rev20160325

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:f4e2a62331ef 1 #include "mbed.h"
sayzyas 0:f4e2a62331ef 2 #include "USBHID.h"
sayzyas 0:f4e2a62331ef 3 #include "USBSerial.h"
sayzyas 0:f4e2a62331ef 4 #include "rtos.h"
sayzyas 0:f4e2a62331ef 5 #include "common.h"
sayzyas 0:f4e2a62331ef 6 #include "TextLCD.h"
sayzyas 0:f4e2a62331ef 7
sayzyas 0:f4e2a62331ef 8 //#include "TextLCD.h"
sayzyas 0:f4e2a62331ef 9
sayzyas 0:f4e2a62331ef 10 //We declare a USBHID device. By default input and output reports are 64 bytes long.
sayzyas 0:f4e2a62331ef 11 USBHID hid(8, 8);
sayzyas 0:f4e2a62331ef 12
sayzyas 0:f4e2a62331ef 13 //USBSerial serial;
sayzyas 0:f4e2a62331ef 14 Serial pc(USBTX, USBRX); // UART
sayzyas 0:f4e2a62331ef 15
sayzyas 0:f4e2a62331ef 16 // I2C i2c(p9, p10); // I2C SDA, SCL
sayzyas 0:f4e2a62331ef 17 I2CSlave slave(p9, p10); //I2C SDA SCL
sayzyas 0:f4e2a62331ef 18
sayzyas 0:f4e2a62331ef 19 //This report will contain data to be sent
sayzyas 0:f4e2a62331ef 20 HID_REPORT send_report;
sayzyas 0:f4e2a62331ef 21 HID_REPORT recv_report;
sayzyas 0:f4e2a62331ef 22
sayzyas 0:f4e2a62331ef 23 DigitalOut led1(LED1); // 1: on, 0: off
sayzyas 0:f4e2a62331ef 24 DigitalOut led2(LED2); // 1: on, 0: off
sayzyas 0:f4e2a62331ef 25 DigitalOut led3(LED3); // 1: on, 0: off
sayzyas 0:f4e2a62331ef 26 DigitalOut led4(LED4); // 1: on, 0: off
sayzyas 0:f4e2a62331ef 27
sayzyas 0:f4e2a62331ef 28 // LED indicator
sayzyas 0:f4e2a62331ef 29 DigitalOut led_ind0(p13);
sayzyas 0:f4e2a62331ef 30 DigitalOut led_ind1(p14);
sayzyas 0:f4e2a62331ef 31 DigitalOut led_ind2(p15);
sayzyas 0:f4e2a62331ef 32 DigitalOut led_ind3(p25);
sayzyas 0:f4e2a62331ef 33 DigitalOut led_ind4(p26);
sayzyas 0:f4e2a62331ef 34 DigitalOut led_ind5(p27);
sayzyas 0:f4e2a62331ef 35 DigitalOut led_ind6(p28);
sayzyas 0:f4e2a62331ef 36 DigitalOut led_ind7(p8);
sayzyas 0:f4e2a62331ef 37
sayzyas 0:f4e2a62331ef 38 // Switch Matrix 3bit SW ON = 0 input
sayzyas 0:f4e2a62331ef 39 // bit 2 1 0 Send data
sayzyas 0:f4e2a62331ef 40 // 0 1 1 1 :
sayzyas 0:f4e2a62331ef 41 // 1 1 1 0 : 0x01 RF Tfm K
sayzyas 0:f4e2a62331ef 42 // 2 1 0 1 : 0x02 RF Tfm I
sayzyas 0:f4e2a62331ef 43 // 3 1 0 0 : 0x04 LB Tfm K
sayzyas 0:f4e2a62331ef 44 // 4 0 1 1 : 0x08 LB Tfm I
sayzyas 0:f4e2a62331ef 45 // 5 0 1 0 : 0x10 Winch Down
sayzyas 0:f4e2a62331ef 46 // 6 0 0 1 : 0x20 Winch Up
sayzyas 0:f4e2a62331ef 47 // 7 0 0 0 :
sayzyas 0:f4e2a62331ef 48 DigitalIn sw1(p5); // Bit 0
sayzyas 0:f4e2a62331ef 49 DigitalIn sw2(p6); // Bit 1
sayzyas 0:f4e2a62331ef 50 DigitalIn sw3(p7); // Bit 2
sayzyas 0:f4e2a62331ef 51
sayzyas 0:f4e2a62331ef 52 // Digital Input 1:OFF, 0:ON
sayzyas 0:f4e2a62331ef 53 DigitalIn sw_jsmode(p30); // SW: Joy Stick mode ( 1: Single, 0: Dual )
sayzyas 0:f4e2a62331ef 54 DigitalIn sw_crexpshape(p29); // SW: CrExp Shape ( 1: KO, 0: I )
sayzyas 0:f4e2a62331ef 55
sayzyas 0:f4e2a62331ef 56 // Analig Joystick input for crawler control
sayzyas 0:f4e2a62331ef 57 //AnalogIn js_l_ud(p20); // Crawler L-UD
sayzyas 0:f4e2a62331ef 58 //AnalogIn js_l_lr(p19); // Crawler L-LR
sayzyas 0:f4e2a62331ef 59 //AnalogIn js_r_ud(p17); // Crawler R-UD
sayzyas 0:f4e2a62331ef 60 //AnalogIn js_r_lr(p16); // Crawler R-LR
sayzyas 0:f4e2a62331ef 61
sayzyas 0:f4e2a62331ef 62 AnalogIn js_l_ud(p19); // Crawler L-UD
sayzyas 0:f4e2a62331ef 63 AnalogIn js_l_lr(p20); // Crawler L-LR
sayzyas 0:f4e2a62331ef 64 AnalogIn js_r_ud(p16); // Crawler R-UD
sayzyas 0:f4e2a62331ef 65 AnalogIn js_r_lr(p17); // Crawler R-LR
sayzyas 0:f4e2a62331ef 66
sayzyas 0:f4e2a62331ef 67 uint8_t js_center_value_r_ud = 0x80;
sayzyas 0:f4e2a62331ef 68 uint8_t js_center_value_r_lr = 0x80;
sayzyas 0:f4e2a62331ef 69 uint8_t js_center_value_l_ud = 0x80;
sayzyas 0:f4e2a62331ef 70 uint8_t js_center_value_l_lr = 0x80;
sayzyas 0:f4e2a62331ef 71
sayzyas 0:f4e2a62331ef 72 void led_demo(void);
sayzyas 0:f4e2a62331ef 73 uint8_t calc_motor_speed( uint8_t data, int32_t dir );
sayzyas 0:f4e2a62331ef 74 void lcd_out( int, int, char* );
sayzyas 0:f4e2a62331ef 75 void calibrate_joystick( int mode, uint8_t data_ud, uint8_t data_lr );
sayzyas 0:f4e2a62331ef 76
sayzyas 0:f4e2a62331ef 77 TextLCD lcd(p11, p12, p24, p23, p22, p21); // rs, e, d4-d7
sayzyas 0:f4e2a62331ef 78
sayzyas 0:f4e2a62331ef 79 int i2c_saddress = 0;
sayzyas 0:f4e2a62331ef 80
sayzyas 0:f4e2a62331ef 81 // LED demo
sayzyas 0:f4e2a62331ef 82 void led_demo()
sayzyas 0:f4e2a62331ef 83 {
sayzyas 0:f4e2a62331ef 84 int i;
sayzyas 0:f4e2a62331ef 85 for( i = 0; i < 7; i++ ) {
sayzyas 0:f4e2a62331ef 86 led_ind0 = 1;
sayzyas 0:f4e2a62331ef 87 led_ind1 = 1;
sayzyas 0:f4e2a62331ef 88 led_ind2 = 1;
sayzyas 0:f4e2a62331ef 89 led_ind3 = 1;
sayzyas 0:f4e2a62331ef 90 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 91 led_ind5 = 1;
sayzyas 0:f4e2a62331ef 92 led_ind6 = 1;
sayzyas 0:f4e2a62331ef 93 led_ind7 = 1;
sayzyas 0:f4e2a62331ef 94 led1 = 1; // on
sayzyas 0:f4e2a62331ef 95 led2 = 0; // off
sayzyas 0:f4e2a62331ef 96 led3 = 0; // off
sayzyas 0:f4e2a62331ef 97 led4 = 0; // off
sayzyas 0:f4e2a62331ef 98 Thread::wait(50);
sayzyas 0:f4e2a62331ef 99 led_ind0 = 0;
sayzyas 0:f4e2a62331ef 100 led_ind1 = 0;
sayzyas 0:f4e2a62331ef 101 led_ind2 = 0;
sayzyas 0:f4e2a62331ef 102 led_ind3 = 0;
sayzyas 0:f4e2a62331ef 103 led_ind4 = 0;
sayzyas 0:f4e2a62331ef 104 led_ind5 = 0;
sayzyas 0:f4e2a62331ef 105 led_ind6 = 0;
sayzyas 0:f4e2a62331ef 106 led_ind7 = 0;
sayzyas 0:f4e2a62331ef 107 led1 = 0; // off
sayzyas 0:f4e2a62331ef 108 led2 = 1; // on
sayzyas 0:f4e2a62331ef 109 led3 = 0; // off
sayzyas 0:f4e2a62331ef 110 led4 = 0; // off
sayzyas 0:f4e2a62331ef 111 Thread::wait(50);
sayzyas 0:f4e2a62331ef 112 led_ind0 = 1;
sayzyas 0:f4e2a62331ef 113 led_ind1 = 1;
sayzyas 0:f4e2a62331ef 114 led_ind2 = 1;
sayzyas 0:f4e2a62331ef 115 led_ind3 = 1;
sayzyas 0:f4e2a62331ef 116 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 117 led_ind5 = 1;
sayzyas 0:f4e2a62331ef 118 led_ind6 = 1;
sayzyas 0:f4e2a62331ef 119 led_ind7 = 1;
sayzyas 0:f4e2a62331ef 120 led1 = 0; // off
sayzyas 0:f4e2a62331ef 121 led2 = 0; // off
sayzyas 0:f4e2a62331ef 122 led3 = 1; // on
sayzyas 0:f4e2a62331ef 123 led4 = 0; // off
sayzyas 0:f4e2a62331ef 124 Thread::wait(50);
sayzyas 0:f4e2a62331ef 125 led_ind0 = 0;
sayzyas 0:f4e2a62331ef 126 led_ind1 = 0;
sayzyas 0:f4e2a62331ef 127 led_ind2 = 0;
sayzyas 0:f4e2a62331ef 128 led_ind3 = 0;
sayzyas 0:f4e2a62331ef 129 led_ind4 = 0;
sayzyas 0:f4e2a62331ef 130 led_ind5 = 0;
sayzyas 0:f4e2a62331ef 131 led_ind6 = 0;
sayzyas 0:f4e2a62331ef 132 led_ind7 = 0;
sayzyas 0:f4e2a62331ef 133 led1 = 0; // off
sayzyas 0:f4e2a62331ef 134 led2 = 0; // off
sayzyas 0:f4e2a62331ef 135 led3 = 0; // off
sayzyas 0:f4e2a62331ef 136 led4 = 1; // on
sayzyas 0:f4e2a62331ef 137 Thread::wait(50);
sayzyas 0:f4e2a62331ef 138 led_ind0 = 1;
sayzyas 0:f4e2a62331ef 139 led_ind1 = 1;
sayzyas 0:f4e2a62331ef 140 led_ind2 = 1;
sayzyas 0:f4e2a62331ef 141 led_ind3 = 1;
sayzyas 0:f4e2a62331ef 142 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 143 led_ind5 = 1;
sayzyas 0:f4e2a62331ef 144 led_ind6 = 1;
sayzyas 0:f4e2a62331ef 145 led_ind7 = 1;
sayzyas 0:f4e2a62331ef 146 led1 = 0; // off
sayzyas 0:f4e2a62331ef 147 led2 = 0; // off
sayzyas 0:f4e2a62331ef 148 led3 = 0; // off
sayzyas 0:f4e2a62331ef 149 led4 = 0; // off
sayzyas 0:f4e2a62331ef 150 }
sayzyas 0:f4e2a62331ef 151 }
sayzyas 0:f4e2a62331ef 152
sayzyas 0:f4e2a62331ef 153 // *****************************************************************
sayzyas 0:f4e2a62331ef 154 // calibration joystick data
sayzyas 0:f4e2a62331ef 155 // *****************************************************************
sayzyas 0:f4e2a62331ef 156 void calibrate_joystick( int mode, uint8_t data_ud, uint8_t data_lr ){
sayzyas 0:f4e2a62331ef 157 uint16_t tmp;
sayzyas 0:f4e2a62331ef 158
sayzyas 0:f4e2a62331ef 159 if( mode == 0 )
sayzyas 0:f4e2a62331ef 160 {
sayzyas 0:f4e2a62331ef 161 tmp = (uint16_t)js_center_value_r_ud;
sayzyas 0:f4e2a62331ef 162 tmp += (uint16_t)data_ud;
sayzyas 0:f4e2a62331ef 163 tmp /= 2;
sayzyas 0:f4e2a62331ef 164 js_center_value_r_ud = (uint8_t)tmp;
sayzyas 0:f4e2a62331ef 165
sayzyas 0:f4e2a62331ef 166 tmp = (uint16_t)js_center_value_r_lr;
sayzyas 0:f4e2a62331ef 167 tmp += (uint16_t)data_lr;
sayzyas 0:f4e2a62331ef 168 tmp /= 2;
sayzyas 0:f4e2a62331ef 169 js_center_value_r_lr = (uint8_t)tmp;
sayzyas 0:f4e2a62331ef 170 }
sayzyas 0:f4e2a62331ef 171 else
sayzyas 0:f4e2a62331ef 172 {
sayzyas 0:f4e2a62331ef 173
sayzyas 0:f4e2a62331ef 174 tmp = (uint16_t)js_center_value_l_ud;
sayzyas 0:f4e2a62331ef 175 tmp += (uint16_t)data_ud;
sayzyas 0:f4e2a62331ef 176 tmp /= 2;
sayzyas 0:f4e2a62331ef 177 js_center_value_l_ud = (uint8_t)tmp;
sayzyas 0:f4e2a62331ef 178
sayzyas 0:f4e2a62331ef 179 tmp = (uint16_t)js_center_value_l_lr;
sayzyas 0:f4e2a62331ef 180 tmp += (uint16_t)data_lr;
sayzyas 0:f4e2a62331ef 181 tmp /= 2;
sayzyas 0:f4e2a62331ef 182 js_center_value_l_lr = (uint8_t)tmp;
sayzyas 0:f4e2a62331ef 183 }
sayzyas 0:f4e2a62331ef 184 }
sayzyas 0:f4e2a62331ef 185
sayzyas 0:f4e2a62331ef 186 // *****************************************************************
sayzyas 0:f4e2a62331ef 187 // calcuilation motor speed
sayzyas 0:f4e2a62331ef 188 // *****************************************************************
sayzyas 0:f4e2a62331ef 189 uint8_t calc_motor_speed( uint8_t data, int32_t dir ){
sayzyas 0:f4e2a62331ef 190 uint8_t speed;
sayzyas 0:f4e2a62331ef 191 if( dir == 0 ){
sayzyas 0:f4e2a62331ef 192 // speed = (data - js_center_value) * 100 / js_center_value;
sayzyas 0:f4e2a62331ef 193 if( speed > 95 ) speed = 100;
sayzyas 0:f4e2a62331ef 194 }
sayzyas 0:f4e2a62331ef 195 else{
sayzyas 0:f4e2a62331ef 196 // speed = (js_center_value - data) * 100 / js_center_value;
sayzyas 0:f4e2a62331ef 197 if( speed > 95 ) speed = 100;
sayzyas 0:f4e2a62331ef 198 }
sayzyas 0:f4e2a62331ef 199 return speed;
sayzyas 0:f4e2a62331ef 200 }
sayzyas 0:f4e2a62331ef 201
sayzyas 0:f4e2a62331ef 202 // *****************************************************************
sayzyas 0:f4e2a62331ef 203 // switch control task
sayzyas 0:f4e2a62331ef 204 // ----------------------
sayzyas 0:f4e2a62331ef 205 //
sayzyas 0:f4e2a62331ef 206 // Up: center -> 255
sayzyas 0:f4e2a62331ef 207 // Down: 002 -> center
sayzyas 0:f4e2a62331ef 208 // Right: center -> 255
sayzyas 0:f4e2a62331ef 209 // Left : 0 -> center
sayzyas 0:f4e2a62331ef 210 //
sayzyas 0:f4e2a62331ef 211 // *****************************************************************
sayzyas 0:f4e2a62331ef 212 void sw_task( void const *){
sayzyas 0:f4e2a62331ef 213
sayzyas 0:f4e2a62331ef 214 char msg[32];
sayzyas 0:f4e2a62331ef 215
sayzyas 0:f4e2a62331ef 216 uint16_t js_rud_data;
sayzyas 0:f4e2a62331ef 217 uint16_t js_rlr_data;
sayzyas 0:f4e2a62331ef 218 uint8_t js_rud_undata;
sayzyas 0:f4e2a62331ef 219 uint8_t js_rlr_undata;
sayzyas 0:f4e2a62331ef 220
sayzyas 0:f4e2a62331ef 221 uint16_t js_lud_data;
sayzyas 0:f4e2a62331ef 222 uint16_t js_llr_data;
sayzyas 0:f4e2a62331ef 223 uint8_t js_lud_undata;
sayzyas 0:f4e2a62331ef 224 uint8_t js_llr_undata;
sayzyas 0:f4e2a62331ef 225
sayzyas 0:f4e2a62331ef 226 float motor_speed_f = 0;
sayzyas 0:f4e2a62331ef 227 uint8_t motor_speed = 0;
sayzyas 0:f4e2a62331ef 228
sayzyas 0:f4e2a62331ef 229 send_report.length = 8;
sayzyas 0:f4e2a62331ef 230
sayzyas 1:368ba89c2e6b 231 send_report.data[0] = 0x80; // L JoyStick Left/Right
sayzyas 0:f4e2a62331ef 232 send_report.data[1] = 0x80; // L JoyStick Up/Down
sayzyas 1:368ba89c2e6b 233 send_report.data[2] = 0x80; // R JoyStick Left/Right^
sayzyas 0:f4e2a62331ef 234 send_report.data[3] = 0x80; // R JoyStick Up/Down
sayzyas 0:f4e2a62331ef 235 send_report.data[4] = 0x00; // Operatiuon Switch
sayzyas 1:368ba89c2e6b 236 // Data Bytes
sayzyas 0:f4e2a62331ef 237 // 7 6 5 4 3 2 1 0
sayzyas 1:368ba89c2e6b 238 // +-+-+-+-+-+----+----+----+----+
sayzyas 1:368ba89c2e6b 239 // |-|-|-|-|-|LB-I|LB-K|RF-I|RF-K|
sayzyas 1:368ba89c2e6b 240 // +-+-+-+-+-+----+----+----+----+
sayzyas 0:f4e2a62331ef 241
sayzyas 0:f4e2a62331ef 242 send_report.data[5] = 0x00; // JS mode switch
sayzyas 1:368ba89c2e6b 243 // Command Bit
sayzyas 0:f4e2a62331ef 244 // 7 6 5 4 3 2 1 0
sayzyas 0:f4e2a62331ef 245 // +-+-+-+-+-+-+---+----+
sayzyas 0:f4e2a62331ef 246 // |-|-|-|-|-|-|S/D|I/KO|
sayzyas 0:f4e2a62331ef 247 // +-+-+-+-+-+-+---+----+
sayzyas 0:f4e2a62331ef 248 // 1:S 1:KO <--- sw off
sayzyas 0:f4e2a62331ef 249 // 0:D 0:I <--- sw on
sayzyas 0:f4e2a62331ef 250
sayzyas 0:f4e2a62331ef 251
sayzyas 0:f4e2a62331ef 252
sayzyas 0:f4e2a62331ef 253 send_report.data[6] = 0x00; // No Function
sayzyas 0:f4e2a62331ef 254 send_report.data[7] = 0x00; // No Function
sayzyas 0:f4e2a62331ef 255 send_report.data[8] = 0x00;
sayzyas 0:f4e2a62331ef 256
sayzyas 0:f4e2a62331ef 257 DEBUG_PRINT_L1("Calibrating joystick ... ");
sayzyas 0:f4e2a62331ef 258 for( int i = 0; i < CALIBRATION_COUNT; i++){
sayzyas 0:f4e2a62331ef 259 js_rud_data = js_r_ud.read_u16();
sayzyas 0:f4e2a62331ef 260 js_rlr_data = js_r_lr.read_u16();
sayzyas 0:f4e2a62331ef 261 js_rud_undata = (uint8_t)(js_rud_data >> 8);
sayzyas 0:f4e2a62331ef 262 js_rlr_undata = (uint8_t)(js_rlr_data >> 8);
sayzyas 0:f4e2a62331ef 263 calibrate_joystick( 0, js_rud_undata, js_rlr_undata); // Under constraction
sayzyas 0:f4e2a62331ef 264 js_lud_data = js_l_ud.read_u16();
sayzyas 0:f4e2a62331ef 265 js_llr_data = js_l_lr.read_u16();
sayzyas 0:f4e2a62331ef 266 js_lud_undata = (uint8_t)(js_lud_data >> 8);
sayzyas 0:f4e2a62331ef 267 js_llr_undata = (uint8_t)(js_llr_data >> 8);
sayzyas 0:f4e2a62331ef 268 calibrate_joystick( 1, js_lud_undata, js_llr_undata); // Under constraction
sayzyas 0:f4e2a62331ef 269 }
sayzyas 0:f4e2a62331ef 270 DEBUG_PRINT_L1("done\r\n");
sayzyas 0:f4e2a62331ef 271
sayzyas 0:f4e2a62331ef 272 while(1){
sayzyas 0:f4e2a62331ef 273 js_rud_data = js_r_ud.read_u16();
sayzyas 0:f4e2a62331ef 274 js_rlr_data = js_r_lr.read_u16();
sayzyas 0:f4e2a62331ef 275 js_lud_data = js_l_ud.read_u16();
sayzyas 0:f4e2a62331ef 276 js_llr_data = js_l_lr.read_u16();
sayzyas 0:f4e2a62331ef 277
sayzyas 0:f4e2a62331ef 278 js_rud_undata = (uint8_t)(js_rud_data >> 8);
sayzyas 0:f4e2a62331ef 279 js_rlr_undata = (uint8_t)(js_rlr_data >> 8);
sayzyas 0:f4e2a62331ef 280 js_lud_undata = (uint8_t)(js_lud_data >> 8);
sayzyas 0:f4e2a62331ef 281 js_llr_undata = (uint8_t)(js_llr_data >> 8);
sayzyas 0:f4e2a62331ef 282
sayzyas 0:f4e2a62331ef 283 DEBUG_PRINT_L1("Handy> ");
sayzyas 0:f4e2a62331ef 284 DEBUG_PRINT_L1("R:%03d/%3d,%03d/%3d ", js_rud_undata, js_center_value_r_ud, js_rlr_undata, js_center_value_r_ud );
sayzyas 0:f4e2a62331ef 285 DEBUG_PRINT_L1("L:%03d/%3d,%03d/%3d ", js_lud_undata, js_center_value_l_ud, js_llr_undata, js_center_value_l_ud );
sayzyas 0:f4e2a62331ef 286 DEBUG_PRINT_L1("SW: %d %d %d ", sw1.read(), sw2.read(), sw3.read() );
sayzyas 0:f4e2a62331ef 287 DEBUG_PRINT_L1("JS: %d %d\r\n", sw_jsmode.read(), sw_crexpshape.read() );
sayzyas 0:f4e2a62331ef 288
sayzyas 0:f4e2a62331ef 289 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 290 // Right JoyStick Control
sayzyas 0:f4e2a62331ef 291 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 292 // Down
sayzyas 0:f4e2a62331ef 293 if( js_rud_undata > (js_center_value_r_ud + DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 294 // Crawler Forward Run
sayzyas 0:f4e2a62331ef 295 led4 = 1;
sayzyas 0:f4e2a62331ef 296 motor_speed = calc_motor_speed( js_rud_undata, 0 );
sayzyas 0:f4e2a62331ef 297 DEBUG_PRINT_L2("Handy> R-JS Up: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rud_undata, js_rud_undata, js_center_value_r_ud);
sayzyas 0:f4e2a62331ef 298 send_report.data[3] = 255 - js_rud_undata; // speed
sayzyas 0:f4e2a62331ef 299 //------------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 300 // sprintf(msg, "R-JS Dn (%03d) ", send_report.data[3]);
sayzyas 0:f4e2a62331ef 301 // lcd_out(0,0,msg);
sayzyas 0:f4e2a62331ef 302 led4 = 0;
sayzyas 0:f4e2a62331ef 303 }
sayzyas 0:f4e2a62331ef 304 // Up
sayzyas 0:f4e2a62331ef 305 else if( js_rud_undata < (js_center_value_r_ud - DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 306 // Crawler Reverse Run
sayzyas 0:f4e2a62331ef 307 led4 = 1;
sayzyas 0:f4e2a62331ef 308 motor_speed = calc_motor_speed( js_rud_undata, 1 );
sayzyas 0:f4e2a62331ef 309 DEBUG_PRINT_L2("Handy> R-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rud_undata, js_rud_undata, js_center_value_r_ud);
sayzyas 0:f4e2a62331ef 310 send_report.data[3] = 255 - js_rud_undata; // speed
sayzyas 0:f4e2a62331ef 311 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 312 // sprintf(msg,"R-JS Up (%03d) ", send_report.data[3]);
sayzyas 0:f4e2a62331ef 313 // lcd_out(0,0,msg);
sayzyas 0:f4e2a62331ef 314 led4 = 0;
sayzyas 0:f4e2a62331ef 315 }
sayzyas 0:f4e2a62331ef 316 else{
sayzyas 0:f4e2a62331ef 317 send_report.data[3] = 0x80; // speed
sayzyas 0:f4e2a62331ef 318 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 319 // sprintf(msg,"R-JS (%03d) ", send_report.data[3]);
sayzyas 0:f4e2a62331ef 320 // lcd_out(0,0,msg);
sayzyas 0:f4e2a62331ef 321 // calibrate_joystick(js_ud_un); // Under constraction
sayzyas 0:f4e2a62331ef 322 led4 = 0;
sayzyas 0:f4e2a62331ef 323 }
sayzyas 0:f4e2a62331ef 324
sayzyas 1:368ba89c2e6b 325 // Right
sayzyas 0:f4e2a62331ef 326 if( js_rlr_undata > (js_center_value_r_ud + DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 327 // Crawler Forward Run
sayzyas 0:f4e2a62331ef 328 led4 = 1;
sayzyas 0:f4e2a62331ef 329 motor_speed = calc_motor_speed( js_rlr_undata, 0 );
sayzyas 0:f4e2a62331ef 330 DEBUG_PRINT_L2("Handy> R-JS Right: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rlr_undata, js_rlr_undata, js_center_value_r_ud);
sayzyas 0:f4e2a62331ef 331 send_report.data[2] = js_rlr_undata; // speed
sayzyas 0:f4e2a62331ef 332 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 333 // sprintf(msg,"R-JS Rt (%03d) ", send_report.data[2]);
sayzyas 0:f4e2a62331ef 334 // lcd_out(0,0,msg);
sayzyas 0:f4e2a62331ef 335 led4 = 0;
sayzyas 0:f4e2a62331ef 336 }
sayzyas 0:f4e2a62331ef 337
sayzyas 0:f4e2a62331ef 338 // Left
sayzyas 0:f4e2a62331ef 339 else if( js_rlr_undata < (js_center_value_r_ud - DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 340 // Crawler Reverse Run
sayzyas 0:f4e2a62331ef 341 led4 = 1;
sayzyas 0:f4e2a62331ef 342 motor_speed = calc_motor_speed( js_rlr_undata, 1 );
sayzyas 0:f4e2a62331ef 343 DEBUG_PRINT_L2("Handy> R-JS Left: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rlr_undata, js_rlr_undata, js_center_value_r_ud);
sayzyas 0:f4e2a62331ef 344 send_report.data[2] = js_rlr_undata; // speed
sayzyas 0:f4e2a62331ef 345 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 346 // sprintf(msg,"R-JS Lt (%03d) ", send_report.data[2]);
sayzyas 0:f4e2a62331ef 347 // lcd_out(0,0,msg);
sayzyas 0:f4e2a62331ef 348 led4 = 0;
sayzyas 0:f4e2a62331ef 349 }
sayzyas 0:f4e2a62331ef 350
sayzyas 0:f4e2a62331ef 351 else{
sayzyas 0:f4e2a62331ef 352 send_report.data[2] = 0x80; // speed
sayzyas 0:f4e2a62331ef 353 // calibrate_joystick(js_ud_un); // Under constraction
sayzyas 0:f4e2a62331ef 354 led4 = 0;
sayzyas 0:f4e2a62331ef 355 }
sayzyas 0:f4e2a62331ef 356
sayzyas 0:f4e2a62331ef 357 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 358 // Left JoyStick Control
sayzyas 0:f4e2a62331ef 359 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 360 // Down
sayzyas 0:f4e2a62331ef 361 if( js_lud_undata > (js_center_value_l_ud + DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 362 // Crawler Forward Run
sayzyas 0:f4e2a62331ef 363 led4 = 1;
sayzyas 0:f4e2a62331ef 364 motor_speed = calc_motor_speed( js_lud_undata, 0 );
sayzyas 0:f4e2a62331ef 365 DEBUG_PRINT_L2("Handy> L-JS Up: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_lud_undata, js_lud_undata, js_center_value_l_ud);
sayzyas 0:f4e2a62331ef 366 send_report.data[1] = 255 - js_lud_undata; // speed
sayzyas 0:f4e2a62331ef 367 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 368 // sprintf(msg,"L-JS Dn (%03d) ", send_report.data[1]);
sayzyas 0:f4e2a62331ef 369 // lcd_out(0,1,msg);
sayzyas 0:f4e2a62331ef 370 led4 = 0;
sayzyas 0:f4e2a62331ef 371 }
sayzyas 0:f4e2a62331ef 372 // Up
sayzyas 0:f4e2a62331ef 373 else if( js_lud_undata < (js_center_value_l_ud - DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 374 // Crawler Reverse Run
sayzyas 0:f4e2a62331ef 375 led4 = 1;
sayzyas 0:f4e2a62331ef 376 motor_speed = calc_motor_speed( js_lud_undata, 1 );
sayzyas 0:f4e2a62331ef 377 DEBUG_PRINT_L2("Handy> L-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_lud_undata, js_lud_undata, js_center_value_l_ud);
sayzyas 0:f4e2a62331ef 378 send_report.data[1] = 255 - js_lud_undata; // speed
sayzyas 0:f4e2a62331ef 379 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 380 // sprintf(msg,"L-JS Up (%03d) ", send_report.data[1]);
sayzyas 0:f4e2a62331ef 381 // lcd_out(0,1,msg);
sayzyas 0:f4e2a62331ef 382 led4 = 0;
sayzyas 0:f4e2a62331ef 383 }
sayzyas 0:f4e2a62331ef 384 else{
sayzyas 0:f4e2a62331ef 385 send_report.data[1] = 0x80; // speed
sayzyas 0:f4e2a62331ef 386 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 387 // sprintf(msg,"L-JS (%03d) ", send_report.data[1]);
sayzyas 0:f4e2a62331ef 388 // lcd_out(0,1,msg);
sayzyas 0:f4e2a62331ef 389 // calibrate_joystick(js_l_ud_un); // Under constraction
sayzyas 0:f4e2a62331ef 390 led4 = 0;
sayzyas 0:f4e2a62331ef 391 }
sayzyas 0:f4e2a62331ef 392
sayzyas 0:f4e2a62331ef 393 // Right
sayzyas 0:f4e2a62331ef 394 if( js_llr_undata > (js_center_value_l_ud + DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 395 // Crawler Forward Run
sayzyas 0:f4e2a62331ef 396 led4 = 1;
sayzyas 0:f4e2a62331ef 397 motor_speed = calc_motor_speed( js_llr_undata, 0 );
sayzyas 0:f4e2a62331ef 398 DEBUG_PRINT_L2("Handy> L-JS Right: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_llr_undata, js_llr_undata, js_center_value_l_ud);
sayzyas 0:f4e2a62331ef 399 send_report.data[0] = js_llr_undata; // speed
sayzyas 0:f4e2a62331ef 400 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 401 // sprintf(msg,"L-JS Rt (%03d) ", send_report.data[0]);
sayzyas 0:f4e2a62331ef 402 // lcd_out(0,1,msg);
sayzyas 0:f4e2a62331ef 403 led4 = 0;
sayzyas 0:f4e2a62331ef 404 }
sayzyas 0:f4e2a62331ef 405 // Left
sayzyas 0:f4e2a62331ef 406 else if( js_llr_undata < (js_center_value_l_ud - DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 407 // Crawler Reverse Run
sayzyas 0:f4e2a62331ef 408 led4 = 1;
sayzyas 0:f4e2a62331ef 409 motor_speed = calc_motor_speed( js_llr_undata, 1 );
sayzyas 0:f4e2a62331ef 410 DEBUG_PRINT_L2("Handy> L-JS Left: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_llr_undata, js_llr_undata, js_center_value_l_ud);
sayzyas 0:f4e2a62331ef 411 send_report.data[0] = js_llr_undata; // speed
sayzyas 0:f4e2a62331ef 412 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 413 // sprintf(msg,"L-JS Lt (%03d) ", send_report.data[0]);
sayzyas 0:f4e2a62331ef 414 // lcd_out(0,1,msg);
sayzyas 0:f4e2a62331ef 415 led4 = 0;
sayzyas 0:f4e2a62331ef 416 }
sayzyas 0:f4e2a62331ef 417
sayzyas 0:f4e2a62331ef 418 else{
sayzyas 0:f4e2a62331ef 419 send_report.data[0] = 0x80; // speed
sayzyas 0:f4e2a62331ef 420 // calibrate_joystick(js_l_ud_un); // Under constraction
sayzyas 0:f4e2a62331ef 421 led4 = 0;
sayzyas 0:f4e2a62331ef 422 }
sayzyas 0:f4e2a62331ef 423
sayzyas 0:f4e2a62331ef 424 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 425 // Other Switch Control: Pass
sayzyas 0:f4e2a62331ef 426 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 427 if ((sw1 == 0)&&(sw2 == 1)&&(sw3==1)){
sayzyas 0:f4e2a62331ef 428 led3 = 1;
sayzyas 0:f4e2a62331ef 429 send_report.data[4] = 0x01; // RF Tfm K (Fwd)
sayzyas 0:f4e2a62331ef 430 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 431 led_ind5 = 1;
sayzyas 1:368ba89c2e6b 432 led_ind6 = 1;
sayzyas 0:f4e2a62331ef 433 }
sayzyas 0:f4e2a62331ef 434 else if ((sw1 == 1)&&(sw2 == 0)&&(sw3==1)){
sayzyas 0:f4e2a62331ef 435 send_report.data[4] = 0x02; // RF Tfm I (Rvs)
sayzyas 0:f4e2a62331ef 436 led3 = 1;
sayzyas 0:f4e2a62331ef 437 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 438 led_ind5 = 1;
sayzyas 1:368ba89c2e6b 439 led_ind6 = 1;
sayzyas 0:f4e2a62331ef 440 }
sayzyas 0:f4e2a62331ef 441 else if ((sw1 == 0)&&(sw2 == 0)&&(sw3==1)){
sayzyas 0:f4e2a62331ef 442 send_report.data[4] = 0x04; // LB Tfm K (Fwd)
sayzyas 0:f4e2a62331ef 443 led3 = 1;
sayzyas 0:f4e2a62331ef 444 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 445 led_ind5 = 1;
sayzyas 1:368ba89c2e6b 446 led_ind6 = 1;
sayzyas 0:f4e2a62331ef 447 }
sayzyas 0:f4e2a62331ef 448 else if ((sw1 == 1)&&(sw2 == 1)&&(sw3==0)){
sayzyas 0:f4e2a62331ef 449 led3 = 1;
sayzyas 0:f4e2a62331ef 450 send_report.data[4] = 0x08; // LB Tfm I (Rvs)
sayzyas 0:f4e2a62331ef 451 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 452 led_ind5 = 1;
sayzyas 1:368ba89c2e6b 453 led_ind6 = 1;
sayzyas 0:f4e2a62331ef 454 }
sayzyas 0:f4e2a62331ef 455 else if ((sw1 == 1)&&(sw2 == 0)&&(sw3==0)){
sayzyas 0:f4e2a62331ef 456 led2 = 1;
sayzyas 0:f4e2a62331ef 457 send_report.data[4] = 0x10; // Winch Down (Fwd)
sayzyas 0:f4e2a62331ef 458 led_ind0 = 1;
sayzyas 0:f4e2a62331ef 459 led_ind1 = 1;
sayzyas 1:368ba89c2e6b 460 led_ind2 = 1;
sayzyas 0:f4e2a62331ef 461 }
sayzyas 0:f4e2a62331ef 462 else if ((sw1 == 0)&&(sw2 == 1)&&(sw3==0)){
sayzyas 0:f4e2a62331ef 463 led2 = 1;
sayzyas 0:f4e2a62331ef 464 send_report.data[4] = 0x20; // Winch Up (Rvs)
sayzyas 0:f4e2a62331ef 465 led_ind0 = 1;
sayzyas 0:f4e2a62331ef 466 led_ind1 = 1;
sayzyas 1:368ba89c2e6b 467 led_ind2 = 1;
sayzyas 0:f4e2a62331ef 468 }
sayzyas 0:f4e2a62331ef 469 else{
sayzyas 0:f4e2a62331ef 470 send_report.data[4] = 0;
sayzyas 0:f4e2a62331ef 471 led_ind0 = 0;
sayzyas 0:f4e2a62331ef 472 led_ind1 = 0;
sayzyas 0:f4e2a62331ef 473 led_ind2 = 0;
sayzyas 0:f4e2a62331ef 474 led_ind3 = 0;
sayzyas 0:f4e2a62331ef 475 led_ind4 = 0;
sayzyas 0:f4e2a62331ef 476 led_ind5 = 0;
sayzyas 0:f4e2a62331ef 477 led_ind6 = 0;
sayzyas 0:f4e2a62331ef 478 led_ind7 = 0;
sayzyas 0:f4e2a62331ef 479 led2 = 0;
sayzyas 0:f4e2a62331ef 480 led3 = 0;
sayzyas 0:f4e2a62331ef 481 }
sayzyas 1:368ba89c2e6b 482 // Command Bit
sayzyas 0:f4e2a62331ef 483 // 7 6 5 4 3 2 1 0
sayzyas 0:f4e2a62331ef 484 // +-+-+-+-+-+-+---+----+
sayzyas 0:f4e2a62331ef 485 // |-|-|-|-|-|-|S/D|I/KO|
sayzyas 0:f4e2a62331ef 486 // +-+-+-+-+-+-+---+----+
sayzyas 0:f4e2a62331ef 487 // 1:S 1:KO <--- sw off
sayzyas 0:f4e2a62331ef 488 // 0:D 0:I <--- sw on
sayzyas 0:f4e2a62331ef 489 if( sw_crexpshape == 1 ){ // sw off
sayzyas 0:f4e2a62331ef 490 send_report.data[5] |= 0x01; // SW off: K-Shape
sayzyas 0:f4e2a62331ef 491 }
sayzyas 0:f4e2a62331ef 492 else{ // sw off
sayzyas 0:f4e2a62331ef 493 send_report.data[5] &= 0xFE; // SW on: I-Shape
sayzyas 0:f4e2a62331ef 494 }
sayzyas 0:f4e2a62331ef 495
sayzyas 0:f4e2a62331ef 496 if( sw_jsmode == 1 ){ // sw off
sayzyas 0:f4e2a62331ef 497 send_report.data[5] |= 0x02; // SW off: Single
sayzyas 1:368ba89c2e6b 498
sayzyas 0:f4e2a62331ef 499 }
sayzyas 0:f4e2a62331ef 500 else{ // sw off
sayzyas 0:f4e2a62331ef 501 send_report.data[5] &= 0xFD; // SW: on Dual
sayzyas 0:f4e2a62331ef 502 }
sayzyas 0:f4e2a62331ef 503
sayzyas 0:f4e2a62331ef 504 // *************************
sayzyas 0:f4e2a62331ef 505 // Send Command to HID host: Hot Plug !!
sayzyas 0:f4e2a62331ef 506 // *************************
sayzyas 0:f4e2a62331ef 507 hid.send(&send_report); // send command to USB HID host
sayzyas 0:f4e2a62331ef 508
sayzyas 0:f4e2a62331ef 509 Thread::wait(5);
sayzyas 1:368ba89c2e6b 510 //lcd_out( 0, 1, "**********" ); // Handy Only
sayzyas 0:f4e2a62331ef 511 }
sayzyas 0:f4e2a62331ef 512
sayzyas 0:f4e2a62331ef 513 }
sayzyas 0:f4e2a62331ef 514
sayzyas 0:f4e2a62331ef 515 void lcd_out( int column, int row, char* msg ){
sayzyas 0:f4e2a62331ef 516
sayzyas 0:f4e2a62331ef 517 // lcd.cls();
sayzyas 0:f4e2a62331ef 518 lcd.locate(column,row);
sayzyas 0:f4e2a62331ef 519
sayzyas 0:f4e2a62331ef 520 /*
sayzyas 0:f4e2a62331ef 521 for( int i = 0; i < 16; i++ ){
sayzyas 0:f4e2a62331ef 522 lcd.putc(*msg++);
sayzyas 0:f4e2a62331ef 523 }
sayzyas 0:f4e2a62331ef 524 */
sayzyas 0:f4e2a62331ef 525 lcd.printf( msg );
sayzyas 0:f4e2a62331ef 526 Thread::wait(10);
sayzyas 0:f4e2a62331ef 527 }
sayzyas 0:f4e2a62331ef 528
sayzyas 0:f4e2a62331ef 529
sayzyas 0:f4e2a62331ef 530 // **********************************************************************
sayzyas 0:f4e2a62331ef 531 //
sayzyas 0:f4e2a62331ef 532 // Main Function of this program
sayzyas 0:f4e2a62331ef 533 //
sayzyas 0:f4e2a62331ef 534 // **********************************************************************
sayzyas 0:f4e2a62331ef 535 int main(void) {
sayzyas 0:f4e2a62331ef 536
sayzyas 0:f4e2a62331ef 537 int32_t counter = 0;
sayzyas 0:f4e2a62331ef 538
sayzyas 0:f4e2a62331ef 539 char msg[16];
sayzyas 0:f4e2a62331ef 540 char buf[11];
sayzyas 0:f4e2a62331ef 541 int i;
sayzyas 0:f4e2a62331ef 542
sayzyas 0:f4e2a62331ef 543 pc.baud(115200);
sayzyas 0:f4e2a62331ef 544
sayzyas 0:f4e2a62331ef 545 sw_jsmode.mode( PullUp ); // use internal pullup
sayzyas 0:f4e2a62331ef 546 sw_crexpshape.mode( PullUp ); // use internal pullup
sayzyas 0:f4e2a62331ef 547
sayzyas 0:f4e2a62331ef 548 i2c_saddress = Target_IIC_ADDR; // defined at header
sayzyas 0:f4e2a62331ef 549 i2c_saddress <<= 1;
sayzyas 0:f4e2a62331ef 550 slave.address(i2c_saddress);
sayzyas 0:f4e2a62331ef 551 DEBUG_PRINT_L0("Handy> IIC Address: %02x\r\n",i2c_saddress);
sayzyas 0:f4e2a62331ef 552
sayzyas 0:f4e2a62331ef 553 lcd_out(0,0,"B2DebrisSurveyor");
sayzyas 0:f4e2a62331ef 554 lcd_out(0,1,"Revast Co.,Ltd. ");
sayzyas 0:f4e2a62331ef 555 led_demo();
sayzyas 1:368ba89c2e6b 556 lcd_out(0,0,"Rev1.00 20160218");
sayzyas 0:f4e2a62331ef 557 lcd_out(0,1,"System Booting..");
sayzyas 0:f4e2a62331ef 558
sayzyas 0:f4e2a62331ef 559 DEBUG_PRINT_L0("\r\n");
sayzyas 0:f4e2a62331ef 560 DEBUG_PRINT_L0("Handy> +-----------------------------------------------------------\r\n");
sayzyas 0:f4e2a62331ef 561 DEBUG_PRINT_L0("Handy> | Project: Crawler Explorer for 1F-1 PCV internal inspection\r\n");
sayzyas 0:f4e2a62331ef 562 DEBUG_PRINT_L0("Handy> |---------\r\n");
sayzyas 0:f4e2a62331ef 563 DEBUG_PRINT_L0("Handy> | This is: Handy Controller main program over USB-HID \r\n");
sayzyas 0:f4e2a62331ef 564 DEBUG_PRINT_L0("Handy> | Target MCU: %s\r\n", Targetmbed);
sayzyas 0:f4e2a62331ef 565 DEBUG_PRINT_L0("Handy> | Letest update: %s\r\n", LatestUpDate);
sayzyas 0:f4e2a62331ef 566 DEBUG_PRINT_L0("Handy> | Program Revision: %s\r\n", ProgramRevision);
sayzyas 0:f4e2a62331ef 567 DEBUG_PRINT_L0("Handy> | Author: %s\r\n", Author);
sayzyas 0:f4e2a62331ef 568 DEBUG_PRINT_L0("Handy> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
sayzyas 0:f4e2a62331ef 569 DEBUG_PRINT_L0("Handy> +-----------------------------------------------------------\r\n");
sayzyas 0:f4e2a62331ef 570
sayzyas 0:f4e2a62331ef 571 Thread task1(sw_task, NULL, osPriorityNormal, 256 * 4);
sayzyas 0:f4e2a62331ef 572
sayzyas 0:f4e2a62331ef 573 // 1234567890123456
sayzyas 0:f4e2a62331ef 574 lcd_out(0,0,"Booted up ! ");
sayzyas 0:f4e2a62331ef 575
sayzyas 0:f4e2a62331ef 576 if(hid.readNB(&recv_report)) {
sayzyas 0:f4e2a62331ef 577 for(int i = 1; i < recv_report.length; i++) {
sayzyas 0:f4e2a62331ef 578 DEBUG_PRINT_L0("%d ", recv_report.data[i]);
sayzyas 0:f4e2a62331ef 579 }
sayzyas 0:f4e2a62331ef 580 DEBUG_PRINT_L0("\r\n");
sayzyas 0:f4e2a62331ef 581 }
sayzyas 0:f4e2a62331ef 582 lcd_out(0,0,"B2DebrisSurveyor");
sayzyas 0:f4e2a62331ef 583
sayzyas 0:f4e2a62331ef 584
sayzyas 0:f4e2a62331ef 585 Thread::wait(5);
sayzyas 0:f4e2a62331ef 586
sayzyas 0:f4e2a62331ef 587 while (1) {
sayzyas 0:f4e2a62331ef 588 //try to read a msg
sayzyas 0:f4e2a62331ef 589
sayzyas 0:f4e2a62331ef 590 if(hid.readNB(&recv_report)) {
sayzyas 0:f4e2a62331ef 591 for(int i = 1; i < recv_report.length; i++) {
sayzyas 0:f4e2a62331ef 592 // DEBUG_PRINT("%d ", recv_report.data[i]);
sayzyas 0:f4e2a62331ef 593 // serial.printf("%d ", recv_report.data[i]);
sayzyas 0:f4e2a62331ef 594 }
sayzyas 0:f4e2a62331ef 595 // DEBUG_PRINT("\r\n");
sayzyas 0:f4e2a62331ef 596 // serial.printf("\r\n");
sayzyas 0:f4e2a62331ef 597
sayzyas 0:f4e2a62331ef 598 }
sayzyas 0:f4e2a62331ef 599
sayzyas 0:f4e2a62331ef 600 i = slave.receive();
sayzyas 0:f4e2a62331ef 601 slave.read(buf, NumberOfI2CCommand ); /* リードサイズは何故か10じゃないと変になる */
sayzyas 0:f4e2a62331ef 602 switch (i) {
sayzyas 0:f4e2a62331ef 603 case I2CSlave::NoData:
sayzyas 0:f4e2a62331ef 604 // DEBUG_PRINT_L0("Handy> the slave has not been addressed\r\n");
sayzyas 0:f4e2a62331ef 605 lcd_out(0,1,"System OK[-] ");
sayzyas 0:f4e2a62331ef 606 break;
sayzyas 0:f4e2a62331ef 607 case I2CSlave::ReadAddressed:
sayzyas 0:f4e2a62331ef 608 DEBUG_PRINT_L0("Handy> the master has requested a read from this slave\r\n");
sayzyas 0:f4e2a62331ef 609 slave.write(msg, strlen(msg) + 1); // Includes null char
sayzyas 0:f4e2a62331ef 610 break;
sayzyas 0:f4e2a62331ef 611 case I2CSlave::WriteGeneral:
sayzyas 0:f4e2a62331ef 612 DEBUG_PRINT_L0("Handy> the master is writing to all slave\r\n");
sayzyas 0:f4e2a62331ef 613 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:f4e2a62331ef 614 DEBUG_PRINT_L0("Handy> Read G: %s\r\n", buf);
sayzyas 0:f4e2a62331ef 615 break;
sayzyas 0:f4e2a62331ef 616 case I2CSlave::WriteAddressed:
sayzyas 0:f4e2a62331ef 617 DEBUG_PRINT_L0("Handy> the master is writing to this slave\r\n");
sayzyas 0:f4e2a62331ef 618 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:f4e2a62331ef 619 // DEBUG_PRINT_L0("Handy> Read A: %s\r\n", buf);
sayzyas 0:f4e2a62331ef 620 if( buf[5] == 0x01 ){
sayzyas 0:f4e2a62331ef 621 lcd_out(0,1,"System OK[|] "); // command
sayzyas 0:f4e2a62331ef 622 }
sayzyas 0:f4e2a62331ef 623 else if( buf[5] == 0x04 ){
sayzyas 0:f4e2a62331ef 624 lcd_out(0,1,"ERROR ");
sayzyas 0:f4e2a62331ef 625 }
sayzyas 0:f4e2a62331ef 626 else{
sayzyas 0:f4e2a62331ef 627 lcd_out(0,1,"System OK[/] "); // unknown command
sayzyas 0:f4e2a62331ef 628 }
sayzyas 0:f4e2a62331ef 629 break;
sayzyas 0:f4e2a62331ef 630 }
sayzyas 0:f4e2a62331ef 631 for(int i = 0; i < 10; i++) buf[i] = 0; // Clear buffer
sayzyas 0:f4e2a62331ef 632
sayzyas 0:f4e2a62331ef 633 counter++;
sayzyas 0:f4e2a62331ef 634 if( counter >= 2 ){
sayzyas 0:f4e2a62331ef 635 led1 = !led1;
sayzyas 0:f4e2a62331ef 636 /*
sayzyas 0:f4e2a62331ef 637 led_ind0 = !led_ind0;
sayzyas 0:f4e2a62331ef 638 led_ind1 = !led_ind1;
sayzyas 0:f4e2a62331ef 639 led_ind2 = !led_ind2;
sayzyas 0:f4e2a62331ef 640 led_ind3 = !led_ind3;
sayzyas 0:f4e2a62331ef 641 led_ind4 = !led_ind4;
sayzyas 0:f4e2a62331ef 642 led_ind5 = !led_ind5;
sayzyas 0:f4e2a62331ef 643 led_ind6 = !led_ind6;
sayzyas 0:f4e2a62331ef 644 led_ind7 = !led_ind7;
sayzyas 0:f4e2a62331ef 645 */
sayzyas 0:f4e2a62331ef 646 counter = 0;
sayzyas 0:f4e2a62331ef 647 }
sayzyas 0:f4e2a62331ef 648 Thread::wait(5);
sayzyas 0:f4e2a62331ef 649 }
sayzyas 0:f4e2a62331ef 650 }