z ysaito / Mbed 2 deprecated Pro_B2_2ZNR_HandyCtrler

2018.07.26

Dependencies:   TextLCD USBDevice mbed-rtos mbed

2_main.cpp@0:f4e2a62331ef, 2016-02-16 (annotated)

Committer:
sayzyas
Date:
Tue Feb 16 16:35:14 2016 +0000
Revision:
0:f4e2a62331ef
Child:
1:368ba89c2e6b
Rev0.95 160216;

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 0:f4e2a62331ef 231 send_report.data[0] = 0x80; // L JoyStick Right/Left
sayzyas 0:f4e2a62331ef 232 send_report.data[1] = 0x80; // L JoyStick Up/Down
sayzyas 0:f4e2a62331ef 233 send_report.data[2] = 0x80; // R JoyStick Right/Left
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 0:f4e2a62331ef 236 // 7 6 5 4 3 2 1 0
sayzyas 0:f4e2a62331ef 237 // +-+-+-+-+----+----+----+----+
sayzyas 0:f4e2a62331ef 238 // |-|-|-|-|LB-I|LB-K|RF-I|RF-K|
sayzyas 0:f4e2a62331ef 239 // +-+-+-+-+----+----+----+----+
sayzyas 0:f4e2a62331ef 240
sayzyas 0:f4e2a62331ef 241 send_report.data[5] = 0x00; // JS mode switch
sayzyas 0:f4e2a62331ef 242 // 7 6 5 4 3 2 1 0
sayzyas 0:f4e2a62331ef 243 // +-+-+-+-+-+-+---+----+
sayzyas 0:f4e2a62331ef 244 // |-|-|-|-|-|-|S/D|I/KO|
sayzyas 0:f4e2a62331ef 245 // +-+-+-+-+-+-+---+----+
sayzyas 0:f4e2a62331ef 246 // 1:S 1:KO <--- sw off
sayzyas 0:f4e2a62331ef 247 // 0:D 0:I <--- sw on
sayzyas 0:f4e2a62331ef 248
sayzyas 0:f4e2a62331ef 249
sayzyas 0:f4e2a62331ef 250
sayzyas 0:f4e2a62331ef 251 send_report.data[6] = 0x00; // No Function
sayzyas 0:f4e2a62331ef 252 send_report.data[7] = 0x00; // No Function
sayzyas 0:f4e2a62331ef 253 send_report.data[8] = 0x00;
sayzyas 0:f4e2a62331ef 254
sayzyas 0:f4e2a62331ef 255 DEBUG_PRINT_L1("Calibrating joystick ... ");
sayzyas 0:f4e2a62331ef 256 for( int i = 0; i < CALIBRATION_COUNT; i++){
sayzyas 0:f4e2a62331ef 257 js_rud_data = js_r_ud.read_u16();
sayzyas 0:f4e2a62331ef 258 js_rlr_data = js_r_lr.read_u16();
sayzyas 0:f4e2a62331ef 259 js_rud_undata = (uint8_t)(js_rud_data >> 8);
sayzyas 0:f4e2a62331ef 260 js_rlr_undata = (uint8_t)(js_rlr_data >> 8);
sayzyas 0:f4e2a62331ef 261 calibrate_joystick( 0, js_rud_undata, js_rlr_undata); // Under constraction
sayzyas 0:f4e2a62331ef 262 js_lud_data = js_l_ud.read_u16();
sayzyas 0:f4e2a62331ef 263 js_llr_data = js_l_lr.read_u16();
sayzyas 0:f4e2a62331ef 264 js_lud_undata = (uint8_t)(js_lud_data >> 8);
sayzyas 0:f4e2a62331ef 265 js_llr_undata = (uint8_t)(js_llr_data >> 8);
sayzyas 0:f4e2a62331ef 266 calibrate_joystick( 1, js_lud_undata, js_llr_undata); // Under constraction
sayzyas 0:f4e2a62331ef 267 }
sayzyas 0:f4e2a62331ef 268 DEBUG_PRINT_L1("done\r\n");
sayzyas 0:f4e2a62331ef 269
sayzyas 0:f4e2a62331ef 270 while(1){
sayzyas 0:f4e2a62331ef 271 js_rud_data = js_r_ud.read_u16();
sayzyas 0:f4e2a62331ef 272 js_rlr_data = js_r_lr.read_u16();
sayzyas 0:f4e2a62331ef 273 js_lud_data = js_l_ud.read_u16();
sayzyas 0:f4e2a62331ef 274 js_llr_data = js_l_lr.read_u16();
sayzyas 0:f4e2a62331ef 275
sayzyas 0:f4e2a62331ef 276 js_rud_undata = (uint8_t)(js_rud_data >> 8);
sayzyas 0:f4e2a62331ef 277 js_rlr_undata = (uint8_t)(js_rlr_data >> 8);
sayzyas 0:f4e2a62331ef 278 js_lud_undata = (uint8_t)(js_lud_data >> 8);
sayzyas 0:f4e2a62331ef 279 js_llr_undata = (uint8_t)(js_llr_data >> 8);
sayzyas 0:f4e2a62331ef 280
sayzyas 0:f4e2a62331ef 281 DEBUG_PRINT_L1("Handy> ");
sayzyas 0:f4e2a62331ef 282 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 283 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 284 DEBUG_PRINT_L1("SW: %d %d %d ", sw1.read(), sw2.read(), sw3.read() );
sayzyas 0:f4e2a62331ef 285 DEBUG_PRINT_L1("JS: %d %d\r\n", sw_jsmode.read(), sw_crexpshape.read() );
sayzyas 0:f4e2a62331ef 286
sayzyas 0:f4e2a62331ef 287 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 288 // Right JoyStick Control
sayzyas 0:f4e2a62331ef 289 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 290 // Down
sayzyas 0:f4e2a62331ef 291 if( js_rud_undata > (js_center_value_r_ud + DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 292 // Crawler Forward Run
sayzyas 0:f4e2a62331ef 293 led4 = 1;
sayzyas 0:f4e2a62331ef 294 motor_speed = calc_motor_speed( js_rud_undata, 0 );
sayzyas 0:f4e2a62331ef 295 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 296 send_report.data[3] = 255 - js_rud_undata; // speed
sayzyas 0:f4e2a62331ef 297 //------------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 298 // sprintf(msg, "R-JS Dn (%03d) ", send_report.data[3]);
sayzyas 0:f4e2a62331ef 299 // lcd_out(0,0,msg);
sayzyas 0:f4e2a62331ef 300 led4 = 0;
sayzyas 0:f4e2a62331ef 301 }
sayzyas 0:f4e2a62331ef 302 // Up
sayzyas 0:f4e2a62331ef 303 else if( js_rud_undata < (js_center_value_r_ud - DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 304 // Crawler Reverse Run
sayzyas 0:f4e2a62331ef 305 led4 = 1;
sayzyas 0:f4e2a62331ef 306 motor_speed = calc_motor_speed( js_rud_undata, 1 );
sayzyas 0:f4e2a62331ef 307 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 308 send_report.data[3] = 255 - js_rud_undata; // speed
sayzyas 0:f4e2a62331ef 309 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 310 // sprintf(msg,"R-JS Up (%03d) ", send_report.data[3]);
sayzyas 0:f4e2a62331ef 311 // lcd_out(0,0,msg);
sayzyas 0:f4e2a62331ef 312 led4 = 0;
sayzyas 0:f4e2a62331ef 313 }
sayzyas 0:f4e2a62331ef 314 else{
sayzyas 0:f4e2a62331ef 315 send_report.data[3] = 0x80; // speed
sayzyas 0:f4e2a62331ef 316 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 317 // sprintf(msg,"R-JS (%03d) ", send_report.data[3]);
sayzyas 0:f4e2a62331ef 318 // lcd_out(0,0,msg);
sayzyas 0:f4e2a62331ef 319 // calibrate_joystick(js_ud_un); // Under constraction
sayzyas 0:f4e2a62331ef 320 led4 = 0;
sayzyas 0:f4e2a62331ef 321 }
sayzyas 0:f4e2a62331ef 322
sayzyas 0:f4e2a62331ef 323 // Right
sayzyas 0:f4e2a62331ef 324 if( js_rlr_undata > (js_center_value_r_ud + DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 325 // Crawler Forward Run
sayzyas 0:f4e2a62331ef 326 led4 = 1;
sayzyas 0:f4e2a62331ef 327 motor_speed = calc_motor_speed( js_rlr_undata, 0 );
sayzyas 0:f4e2a62331ef 328 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 329 send_report.data[2] = js_rlr_undata; // speed
sayzyas 0:f4e2a62331ef 330 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 331 // sprintf(msg,"R-JS Rt (%03d) ", send_report.data[2]);
sayzyas 0:f4e2a62331ef 332 // lcd_out(0,0,msg);
sayzyas 0:f4e2a62331ef 333 led4 = 0;
sayzyas 0:f4e2a62331ef 334 }
sayzyas 0:f4e2a62331ef 335
sayzyas 0:f4e2a62331ef 336 // Left
sayzyas 0:f4e2a62331ef 337 else if( js_rlr_undata < (js_center_value_r_ud - DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 338 // Crawler Reverse Run
sayzyas 0:f4e2a62331ef 339 led4 = 1;
sayzyas 0:f4e2a62331ef 340 motor_speed = calc_motor_speed( js_rlr_undata, 1 );
sayzyas 0:f4e2a62331ef 341 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 342 send_report.data[2] = js_rlr_undata; // speed
sayzyas 0:f4e2a62331ef 343 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 344 // sprintf(msg,"R-JS Lt (%03d) ", send_report.data[2]);
sayzyas 0:f4e2a62331ef 345 // lcd_out(0,0,msg);
sayzyas 0:f4e2a62331ef 346 led4 = 0;
sayzyas 0:f4e2a62331ef 347 }
sayzyas 0:f4e2a62331ef 348
sayzyas 0:f4e2a62331ef 349 else{
sayzyas 0:f4e2a62331ef 350 send_report.data[2] = 0x80; // speed
sayzyas 0:f4e2a62331ef 351 // calibrate_joystick(js_ud_un); // Under constraction
sayzyas 0:f4e2a62331ef 352 led4 = 0;
sayzyas 0:f4e2a62331ef 353 }
sayzyas 0:f4e2a62331ef 354
sayzyas 0:f4e2a62331ef 355 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 356 // Left JoyStick Control
sayzyas 0:f4e2a62331ef 357 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 358 // Down
sayzyas 0:f4e2a62331ef 359 if( js_lud_undata > (js_center_value_l_ud + DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 360 // Crawler Forward Run
sayzyas 0:f4e2a62331ef 361 led4 = 1;
sayzyas 0:f4e2a62331ef 362 motor_speed = calc_motor_speed( js_lud_undata, 0 );
sayzyas 0:f4e2a62331ef 363 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 364 send_report.data[1] = 255 - js_lud_undata; // speed
sayzyas 0:f4e2a62331ef 365 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 366 // sprintf(msg,"L-JS Dn (%03d) ", send_report.data[1]);
sayzyas 0:f4e2a62331ef 367 // lcd_out(0,1,msg);
sayzyas 0:f4e2a62331ef 368 led4 = 0;
sayzyas 0:f4e2a62331ef 369 }
sayzyas 0:f4e2a62331ef 370 // Up
sayzyas 0:f4e2a62331ef 371 else if( js_lud_undata < (js_center_value_l_ud - DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 372 // Crawler Reverse Run
sayzyas 0:f4e2a62331ef 373 led4 = 1;
sayzyas 0:f4e2a62331ef 374 motor_speed = calc_motor_speed( js_lud_undata, 1 );
sayzyas 0:f4e2a62331ef 375 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 376 send_report.data[1] = 255 - js_lud_undata; // speed
sayzyas 0:f4e2a62331ef 377 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 378 // sprintf(msg,"L-JS Up (%03d) ", send_report.data[1]);
sayzyas 0:f4e2a62331ef 379 // lcd_out(0,1,msg);
sayzyas 0:f4e2a62331ef 380 led4 = 0;
sayzyas 0:f4e2a62331ef 381 }
sayzyas 0:f4e2a62331ef 382 else{
sayzyas 0:f4e2a62331ef 383 send_report.data[1] = 0x80; // speed
sayzyas 0:f4e2a62331ef 384 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 385 // sprintf(msg,"L-JS (%03d) ", send_report.data[1]);
sayzyas 0:f4e2a62331ef 386 // lcd_out(0,1,msg);
sayzyas 0:f4e2a62331ef 387 // calibrate_joystick(js_l_ud_un); // Under constraction
sayzyas 0:f4e2a62331ef 388 led4 = 0;
sayzyas 0:f4e2a62331ef 389 }
sayzyas 0:f4e2a62331ef 390
sayzyas 0:f4e2a62331ef 391 // Right
sayzyas 0:f4e2a62331ef 392 if( js_llr_undata > (js_center_value_l_ud + DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 393 // Crawler Forward Run
sayzyas 0:f4e2a62331ef 394 led4 = 1;
sayzyas 0:f4e2a62331ef 395 motor_speed = calc_motor_speed( js_llr_undata, 0 );
sayzyas 0:f4e2a62331ef 396 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 397 send_report.data[0] = js_llr_undata; // speed
sayzyas 0:f4e2a62331ef 398 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 399 // sprintf(msg,"L-JS Rt (%03d) ", send_report.data[0]);
sayzyas 0:f4e2a62331ef 400 // lcd_out(0,1,msg);
sayzyas 0:f4e2a62331ef 401 led4 = 0;
sayzyas 0:f4e2a62331ef 402 }
sayzyas 0:f4e2a62331ef 403 // Left
sayzyas 0:f4e2a62331ef 404 else if( js_llr_undata < (js_center_value_l_ud - DEAD_ZONE_BAND_WIDTH) ){
sayzyas 0:f4e2a62331ef 405 // Crawler Reverse Run
sayzyas 0:f4e2a62331ef 406 led4 = 1;
sayzyas 0:f4e2a62331ef 407 motor_speed = calc_motor_speed( js_llr_undata, 1 );
sayzyas 0:f4e2a62331ef 408 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 409 send_report.data[0] = js_llr_undata; // speed
sayzyas 0:f4e2a62331ef 410 //-----------123456789 123456--------------------
sayzyas 0:f4e2a62331ef 411 // sprintf(msg,"L-JS Lt (%03d) ", send_report.data[0]);
sayzyas 0:f4e2a62331ef 412 // lcd_out(0,1,msg);
sayzyas 0:f4e2a62331ef 413 led4 = 0;
sayzyas 0:f4e2a62331ef 414 }
sayzyas 0:f4e2a62331ef 415
sayzyas 0:f4e2a62331ef 416 else{
sayzyas 0:f4e2a62331ef 417 send_report.data[0] = 0x80; // speed
sayzyas 0:f4e2a62331ef 418 // calibrate_joystick(js_l_ud_un); // Under constraction
sayzyas 0:f4e2a62331ef 419 led4 = 0;
sayzyas 0:f4e2a62331ef 420 }
sayzyas 0:f4e2a62331ef 421
sayzyas 0:f4e2a62331ef 422 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 423 // Other Switch Control: Pass
sayzyas 0:f4e2a62331ef 424 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 425 if ((sw1 == 0)&&(sw2 == 1)&&(sw3==1)){
sayzyas 0:f4e2a62331ef 426 led3 = 1;
sayzyas 0:f4e2a62331ef 427 send_report.data[4] = 0x01; // RF Tfm K (Fwd)
sayzyas 0:f4e2a62331ef 428 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 429 led_ind5 = 1;
sayzyas 0:f4e2a62331ef 430 // led3 = 0;
sayzyas 0:f4e2a62331ef 431 }
sayzyas 0:f4e2a62331ef 432 else if ((sw1 == 1)&&(sw2 == 0)&&(sw3==1)){
sayzyas 0:f4e2a62331ef 433 send_report.data[4] = 0x02; // RF Tfm I (Rvs)
sayzyas 0:f4e2a62331ef 434 led3 = 1;
sayzyas 0:f4e2a62331ef 435 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 436 led_ind5 = 1;
sayzyas 0:f4e2a62331ef 437 // led3 = 0;
sayzyas 0:f4e2a62331ef 438 }
sayzyas 0:f4e2a62331ef 439 else if ((sw1 == 0)&&(sw2 == 0)&&(sw3==1)){
sayzyas 0:f4e2a62331ef 440 send_report.data[4] = 0x04; // LB Tfm K (Fwd)
sayzyas 0:f4e2a62331ef 441 led3 = 1;
sayzyas 0:f4e2a62331ef 442 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 443 led_ind5 = 1;
sayzyas 0:f4e2a62331ef 444 // led3 = 0;
sayzyas 0:f4e2a62331ef 445 }
sayzyas 0:f4e2a62331ef 446 else if ((sw1 == 1)&&(sw2 == 1)&&(sw3==0)){
sayzyas 0:f4e2a62331ef 447 led3 = 1;
sayzyas 0:f4e2a62331ef 448 send_report.data[4] = 0x08; // LB Tfm I (Rvs)
sayzyas 0:f4e2a62331ef 449 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 450 led_ind5 = 1;
sayzyas 0:f4e2a62331ef 451 // led3 = 0;
sayzyas 0:f4e2a62331ef 452 }
sayzyas 0:f4e2a62331ef 453 else if ((sw1 == 1)&&(sw2 == 0)&&(sw3==0)){
sayzyas 0:f4e2a62331ef 454 led2 = 1;
sayzyas 0:f4e2a62331ef 455 send_report.data[4] = 0x10; // Winch Down (Fwd)
sayzyas 0:f4e2a62331ef 456 led_ind0 = 1;
sayzyas 0:f4e2a62331ef 457 led_ind1 = 1;
sayzyas 0:f4e2a62331ef 458 // led3 = 0;
sayzyas 0:f4e2a62331ef 459 }
sayzyas 0:f4e2a62331ef 460 else if ((sw1 == 0)&&(sw2 == 1)&&(sw3==0)){
sayzyas 0:f4e2a62331ef 461 led2 = 1;
sayzyas 0:f4e2a62331ef 462 send_report.data[4] = 0x20; // Winch Up (Rvs)
sayzyas 0:f4e2a62331ef 463 led_ind0 = 1;
sayzyas 0:f4e2a62331ef 464 led_ind1 = 1;
sayzyas 0:f4e2a62331ef 465 // led3 = 0;
sayzyas 0:f4e2a62331ef 466 }
sayzyas 0:f4e2a62331ef 467 else{
sayzyas 0:f4e2a62331ef 468 send_report.data[4] = 0;
sayzyas 0:f4e2a62331ef 469 led_ind0 = 0;
sayzyas 0:f4e2a62331ef 470 led_ind1 = 0;
sayzyas 0:f4e2a62331ef 471 led_ind2 = 0;
sayzyas 0:f4e2a62331ef 472 led_ind3 = 0;
sayzyas 0:f4e2a62331ef 473 led_ind4 = 0;
sayzyas 0:f4e2a62331ef 474 led_ind5 = 0;
sayzyas 0:f4e2a62331ef 475 led_ind6 = 0;
sayzyas 0:f4e2a62331ef 476 led_ind7 = 0;
sayzyas 0:f4e2a62331ef 477 led2 = 0;
sayzyas 0:f4e2a62331ef 478 led3 = 0;
sayzyas 0:f4e2a62331ef 479 }
sayzyas 0:f4e2a62331ef 480
sayzyas 0:f4e2a62331ef 481 // 7 6 5 4 3 2 1 0
sayzyas 0:f4e2a62331ef 482 // +-+-+-+-+-+-+---+----+
sayzyas 0:f4e2a62331ef 483 // |-|-|-|-|-|-|S/D|I/KO|
sayzyas 0:f4e2a62331ef 484 // +-+-+-+-+-+-+---+----+
sayzyas 0:f4e2a62331ef 485 // 1:S 1:KO <--- sw off
sayzyas 0:f4e2a62331ef 486 // 0:D 0:I <--- sw on
sayzyas 0:f4e2a62331ef 487 if( sw_crexpshape == 1 ){ // sw off
sayzyas 0:f4e2a62331ef 488 send_report.data[5] |= 0x01; // SW off: K-Shape
sayzyas 0:f4e2a62331ef 489 }
sayzyas 0:f4e2a62331ef 490 else{ // sw off
sayzyas 0:f4e2a62331ef 491 send_report.data[5] &= 0xFE; // SW on: I-Shape
sayzyas 0:f4e2a62331ef 492 }
sayzyas 0:f4e2a62331ef 493
sayzyas 0:f4e2a62331ef 494 if( sw_jsmode == 1 ){ // sw off
sayzyas 0:f4e2a62331ef 495 send_report.data[5] |= 0x02; // SW off: Single
sayzyas 0:f4e2a62331ef 496 }
sayzyas 0:f4e2a62331ef 497 else{ // sw off
sayzyas 0:f4e2a62331ef 498 send_report.data[5] &= 0xFD; // SW: on Dual
sayzyas 0:f4e2a62331ef 499 }
sayzyas 0:f4e2a62331ef 500
sayzyas 0:f4e2a62331ef 501 // *************************
sayzyas 0:f4e2a62331ef 502 // Send Command to HID host: Hot Plug !!
sayzyas 0:f4e2a62331ef 503 // *************************
sayzyas 0:f4e2a62331ef 504 hid.send(&send_report); // send command to USB HID host
sayzyas 0:f4e2a62331ef 505
sayzyas 0:f4e2a62331ef 506 Thread::wait(5);
sayzyas 0:f4e2a62331ef 507 }
sayzyas 0:f4e2a62331ef 508
sayzyas 0:f4e2a62331ef 509 }
sayzyas 0:f4e2a62331ef 510
sayzyas 0:f4e2a62331ef 511 void lcd_out( int column, int row, char* msg ){
sayzyas 0:f4e2a62331ef 512
sayzyas 0:f4e2a62331ef 513 // lcd.cls();
sayzyas 0:f4e2a62331ef 514 lcd.locate(column,row);
sayzyas 0:f4e2a62331ef 515
sayzyas 0:f4e2a62331ef 516 /*
sayzyas 0:f4e2a62331ef 517 for( int i = 0; i < 16; i++ ){
sayzyas 0:f4e2a62331ef 518 lcd.putc(*msg++);
sayzyas 0:f4e2a62331ef 519 }
sayzyas 0:f4e2a62331ef 520 */
sayzyas 0:f4e2a62331ef 521 lcd.printf( msg );
sayzyas 0:f4e2a62331ef 522 Thread::wait(10);
sayzyas 0:f4e2a62331ef 523 }
sayzyas 0:f4e2a62331ef 524
sayzyas 0:f4e2a62331ef 525
sayzyas 0:f4e2a62331ef 526 // **********************************************************************
sayzyas 0:f4e2a62331ef 527 //
sayzyas 0:f4e2a62331ef 528 // Main Function of this program
sayzyas 0:f4e2a62331ef 529 //
sayzyas 0:f4e2a62331ef 530 // **********************************************************************
sayzyas 0:f4e2a62331ef 531 int main(void) {
sayzyas 0:f4e2a62331ef 532
sayzyas 0:f4e2a62331ef 533 int32_t counter = 0;
sayzyas 0:f4e2a62331ef 534
sayzyas 0:f4e2a62331ef 535 char msg[16];
sayzyas 0:f4e2a62331ef 536 char buf[11];
sayzyas 0:f4e2a62331ef 537 int i;
sayzyas 0:f4e2a62331ef 538
sayzyas 0:f4e2a62331ef 539 pc.baud(115200);
sayzyas 0:f4e2a62331ef 540
sayzyas 0:f4e2a62331ef 541 sw_jsmode.mode( PullUp ); // use internal pullup
sayzyas 0:f4e2a62331ef 542 sw_crexpshape.mode( PullUp ); // use internal pullup
sayzyas 0:f4e2a62331ef 543
sayzyas 0:f4e2a62331ef 544 i2c_saddress = Target_IIC_ADDR; // defined at header
sayzyas 0:f4e2a62331ef 545 i2c_saddress <<= 1;
sayzyas 0:f4e2a62331ef 546 slave.address(i2c_saddress);
sayzyas 0:f4e2a62331ef 547 DEBUG_PRINT_L0("Handy> IIC Address: %02x\r\n",i2c_saddress);
sayzyas 0:f4e2a62331ef 548
sayzyas 0:f4e2a62331ef 549 lcd_out(0,0,"B2DebrisSurveyor");
sayzyas 0:f4e2a62331ef 550 lcd_out(0,1,"Revast Co.,Ltd. ");
sayzyas 0:f4e2a62331ef 551 led_demo();
sayzyas 0:f4e2a62331ef 552 lcd_out(0,0,"Rev1.00 20160201");
sayzyas 0:f4e2a62331ef 553 lcd_out(0,1,"System Booting..");
sayzyas 0:f4e2a62331ef 554
sayzyas 0:f4e2a62331ef 555 DEBUG_PRINT_L0("\r\n");
sayzyas 0:f4e2a62331ef 556 DEBUG_PRINT_L0("Handy> +-----------------------------------------------------------\r\n");
sayzyas 0:f4e2a62331ef 557 DEBUG_PRINT_L0("Handy> | Project: Crawler Explorer for 1F-1 PCV internal inspection\r\n");
sayzyas 0:f4e2a62331ef 558 DEBUG_PRINT_L0("Handy> |---------\r\n");
sayzyas 0:f4e2a62331ef 559 DEBUG_PRINT_L0("Handy> | This is: Handy Controller main program over USB-HID \r\n");
sayzyas 0:f4e2a62331ef 560 DEBUG_PRINT_L0("Handy> | Target MCU: %s\r\n", Targetmbed);
sayzyas 0:f4e2a62331ef 561 DEBUG_PRINT_L0("Handy> | Letest update: %s\r\n", LatestUpDate);
sayzyas 0:f4e2a62331ef 562 DEBUG_PRINT_L0("Handy> | Program Revision: %s\r\n", ProgramRevision);
sayzyas 0:f4e2a62331ef 563 DEBUG_PRINT_L0("Handy> | Author: %s\r\n", Author);
sayzyas 0:f4e2a62331ef 564 DEBUG_PRINT_L0("Handy> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
sayzyas 0:f4e2a62331ef 565 DEBUG_PRINT_L0("Handy> +-----------------------------------------------------------\r\n");
sayzyas 0:f4e2a62331ef 566
sayzyas 0:f4e2a62331ef 567 Thread task1(sw_task, NULL, osPriorityNormal, 256 * 4);
sayzyas 0:f4e2a62331ef 568
sayzyas 0:f4e2a62331ef 569 // 1234567890123456
sayzyas 0:f4e2a62331ef 570 lcd_out(0,0,"Booted up ! ");
sayzyas 0:f4e2a62331ef 571
sayzyas 0:f4e2a62331ef 572 if(hid.readNB(&recv_report)) {
sayzyas 0:f4e2a62331ef 573 for(int i = 1; i < recv_report.length; i++) {
sayzyas 0:f4e2a62331ef 574 DEBUG_PRINT_L0("%d ", recv_report.data[i]);
sayzyas 0:f4e2a62331ef 575 }
sayzyas 0:f4e2a62331ef 576 DEBUG_PRINT_L0("\r\n");
sayzyas 0:f4e2a62331ef 577 }
sayzyas 0:f4e2a62331ef 578 lcd_out(0,0,"B2DebrisSurveyor");
sayzyas 0:f4e2a62331ef 579
sayzyas 0:f4e2a62331ef 580
sayzyas 0:f4e2a62331ef 581 Thread::wait(5);
sayzyas 0:f4e2a62331ef 582
sayzyas 0:f4e2a62331ef 583 while (1) {
sayzyas 0:f4e2a62331ef 584 //try to read a msg
sayzyas 0:f4e2a62331ef 585
sayzyas 0:f4e2a62331ef 586 if(hid.readNB(&recv_report)) {
sayzyas 0:f4e2a62331ef 587 for(int i = 1; i < recv_report.length; i++) {
sayzyas 0:f4e2a62331ef 588 // DEBUG_PRINT("%d ", recv_report.data[i]);
sayzyas 0:f4e2a62331ef 589 // serial.printf("%d ", recv_report.data[i]);
sayzyas 0:f4e2a62331ef 590 }
sayzyas 0:f4e2a62331ef 591 // DEBUG_PRINT("\r\n");
sayzyas 0:f4e2a62331ef 592 // serial.printf("\r\n");
sayzyas 0:f4e2a62331ef 593
sayzyas 0:f4e2a62331ef 594 }
sayzyas 0:f4e2a62331ef 595
sayzyas 0:f4e2a62331ef 596 i = slave.receive();
sayzyas 0:f4e2a62331ef 597 slave.read(buf, NumberOfI2CCommand ); /* リードサイズは何故か10じゃないと変になる */
sayzyas 0:f4e2a62331ef 598 switch (i) {
sayzyas 0:f4e2a62331ef 599 case I2CSlave::NoData:
sayzyas 0:f4e2a62331ef 600 // DEBUG_PRINT_L0("Handy> the slave has not been addressed\r\n");
sayzyas 0:f4e2a62331ef 601 lcd_out(0,1,"System OK[-] ");
sayzyas 0:f4e2a62331ef 602 break;
sayzyas 0:f4e2a62331ef 603 case I2CSlave::ReadAddressed:
sayzyas 0:f4e2a62331ef 604 DEBUG_PRINT_L0("Handy> the master has requested a read from this slave\r\n");
sayzyas 0:f4e2a62331ef 605 slave.write(msg, strlen(msg) + 1); // Includes null char
sayzyas 0:f4e2a62331ef 606 break;
sayzyas 0:f4e2a62331ef 607 case I2CSlave::WriteGeneral:
sayzyas 0:f4e2a62331ef 608 DEBUG_PRINT_L0("Handy> the master is writing to all slave\r\n");
sayzyas 0:f4e2a62331ef 609 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:f4e2a62331ef 610 DEBUG_PRINT_L0("Handy> Read G: %s\r\n", buf);
sayzyas 0:f4e2a62331ef 611 break;
sayzyas 0:f4e2a62331ef 612 case I2CSlave::WriteAddressed:
sayzyas 0:f4e2a62331ef 613 DEBUG_PRINT_L0("Handy> the master is writing to this slave\r\n");
sayzyas 0:f4e2a62331ef 614 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:f4e2a62331ef 615 // DEBUG_PRINT_L0("Handy> Read A: %s\r\n", buf);
sayzyas 0:f4e2a62331ef 616 if( buf[5] == 0x01 ){
sayzyas 0:f4e2a62331ef 617 lcd_out(0,1,"System OK[|] "); // command
sayzyas 0:f4e2a62331ef 618 }
sayzyas 0:f4e2a62331ef 619 else if( buf[5] == 0x04 ){
sayzyas 0:f4e2a62331ef 620 lcd_out(0,1,"ERROR ");
sayzyas 0:f4e2a62331ef 621 }
sayzyas 0:f4e2a62331ef 622 else{
sayzyas 0:f4e2a62331ef 623 lcd_out(0,1,"System OK[/] "); // unknown command
sayzyas 0:f4e2a62331ef 624 }
sayzyas 0:f4e2a62331ef 625 break;
sayzyas 0:f4e2a62331ef 626 }
sayzyas 0:f4e2a62331ef 627 for(int i = 0; i < 10; i++) buf[i] = 0; // Clear buffer
sayzyas 0:f4e2a62331ef 628
sayzyas 0:f4e2a62331ef 629 counter++;
sayzyas 0:f4e2a62331ef 630 if( counter >= 2 ){
sayzyas 0:f4e2a62331ef 631 led1 = !led1;
sayzyas 0:f4e2a62331ef 632 /*
sayzyas 0:f4e2a62331ef 633 led_ind0 = !led_ind0;
sayzyas 0:f4e2a62331ef 634 led_ind1 = !led_ind1;
sayzyas 0:f4e2a62331ef 635 led_ind2 = !led_ind2;
sayzyas 0:f4e2a62331ef 636 led_ind3 = !led_ind3;
sayzyas 0:f4e2a62331ef 637 led_ind4 = !led_ind4;
sayzyas 0:f4e2a62331ef 638 led_ind5 = !led_ind5;
sayzyas 0:f4e2a62331ef 639 led_ind6 = !led_ind6;
sayzyas 0:f4e2a62331ef 640 led_ind7 = !led_ind7;
sayzyas 0:f4e2a62331ef 641 */
sayzyas 0:f4e2a62331ef 642 counter = 0;
sayzyas 0:f4e2a62331ef 643 }
sayzyas 0:f4e2a62331ef 644 Thread::wait(5);
sayzyas 0:f4e2a62331ef 645 }
sayzyas 0:f4e2a62331ef 646 }