z ysaito / Mbed 2 deprecated Pro_B2_2ZNR_HandyCtrler

2018.07.26

Dependencies:   TextLCD USBDevice mbed-rtos mbed

2_main.cpp@2:d0a5ee4e7bb8, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 00:20:57 2018 +0000
Revision:
2:d0a5ee4e7bb8
Parent:
1:368ba89c2e6b 
2018.07.26

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 2:d0a5ee4e7bb8 53 DigitalIn sw_ikmode(p29); // SW: i \ k ,pde ( 1: K, 0: I )
sayzyas 2:d0a5ee4e7bb8 54 DigitalIn sw_validpart(p30); // SW: CrExp Shape ( 1: Left-Winch. 0: Right-tfmcrwler )
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 2:d0a5ee4e7bb8 75 void lcd_out2( int column, int row, char* msg, int cnt);
sayzyas 2:d0a5ee4e7bb8 76
sayzyas 0:f4e2a62331ef 77 void calibrate_joystick( int mode, uint8_t data_ud, uint8_t data_lr );
sayzyas 0:f4e2a62331ef 78
sayzyas 0:f4e2a62331ef 79 TextLCD lcd(p11, p12, p24, p23, p22, p21); // rs, e, d4-d7
sayzyas 0:f4e2a62331ef 80
sayzyas 0:f4e2a62331ef 81 int i2c_saddress = 0;
sayzyas 0:f4e2a62331ef 82
sayzyas 2:d0a5ee4e7bb8 83 //
sayzyas 2:d0a5ee4e7bb8 84 void led_winch_valid( int counter )
sayzyas 2:d0a5ee4e7bb8 85 {
sayzyas 2:d0a5ee4e7bb8 86 if( counter < 25)
sayzyas 2:d0a5ee4e7bb8 87 {
sayzyas 2:d0a5ee4e7bb8 88 led_ind0 = 0;
sayzyas 2:d0a5ee4e7bb8 89 led_ind1 = 0;
sayzyas 2:d0a5ee4e7bb8 90 led_ind2 = 0;
sayzyas 2:d0a5ee4e7bb8 91 led_ind3 = 0;
sayzyas 2:d0a5ee4e7bb8 92 }
sayzyas 2:d0a5ee4e7bb8 93 else
sayzyas 2:d0a5ee4e7bb8 94 {
sayzyas 2:d0a5ee4e7bb8 95 led_ind0 = 1;
sayzyas 2:d0a5ee4e7bb8 96 led_ind1 = 1;
sayzyas 2:d0a5ee4e7bb8 97 led_ind2 = 1;
sayzyas 2:d0a5ee4e7bb8 98 led_ind3 = 1;
sayzyas 2:d0a5ee4e7bb8 99 }
sayzyas 2:d0a5ee4e7bb8 100 }
sayzyas 2:d0a5ee4e7bb8 101
sayzyas 2:d0a5ee4e7bb8 102 void led_crawler_valid( int counter )
sayzyas 2:d0a5ee4e7bb8 103 {
sayzyas 2:d0a5ee4e7bb8 104 if( counter < 25)
sayzyas 2:d0a5ee4e7bb8 105 {
sayzyas 2:d0a5ee4e7bb8 106 led_ind4 = 0;
sayzyas 2:d0a5ee4e7bb8 107 led_ind5 = 0;
sayzyas 2:d0a5ee4e7bb8 108 led_ind6 = 0;
sayzyas 2:d0a5ee4e7bb8 109 led_ind7 = 0;
sayzyas 2:d0a5ee4e7bb8 110 }
sayzyas 2:d0a5ee4e7bb8 111 else
sayzyas 2:d0a5ee4e7bb8 112 {
sayzyas 2:d0a5ee4e7bb8 113 led_ind4 = 1;
sayzyas 2:d0a5ee4e7bb8 114 led_ind5 = 1;
sayzyas 2:d0a5ee4e7bb8 115 led_ind6 = 1;
sayzyas 2:d0a5ee4e7bb8 116 led_ind7 = 1;
sayzyas 2:d0a5ee4e7bb8 117 }
sayzyas 2:d0a5ee4e7bb8 118 }
sayzyas 2:d0a5ee4e7bb8 119
sayzyas 0:f4e2a62331ef 120 // LED demo
sayzyas 0:f4e2a62331ef 121 void led_demo()
sayzyas 0:f4e2a62331ef 122 {
sayzyas 0:f4e2a62331ef 123 int i;
sayzyas 0:f4e2a62331ef 124 for( i = 0; i < 7; i++ ) {
sayzyas 0:f4e2a62331ef 125 led_ind0 = 1;
sayzyas 0:f4e2a62331ef 126 led_ind1 = 1;
sayzyas 0:f4e2a62331ef 127 led_ind2 = 1;
sayzyas 0:f4e2a62331ef 128 led_ind3 = 1;
sayzyas 0:f4e2a62331ef 129 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 130 led_ind5 = 1;
sayzyas 0:f4e2a62331ef 131 led_ind6 = 1;
sayzyas 0:f4e2a62331ef 132 led_ind7 = 1;
sayzyas 0:f4e2a62331ef 133 led1 = 1; // on
sayzyas 0:f4e2a62331ef 134 led2 = 0; // off
sayzyas 0:f4e2a62331ef 135 led3 = 0; // off
sayzyas 0:f4e2a62331ef 136 led4 = 0; // off
sayzyas 2:d0a5ee4e7bb8 137 Thread::wait(15);
sayzyas 0:f4e2a62331ef 138 led_ind0 = 0;
sayzyas 0:f4e2a62331ef 139 led_ind1 = 0;
sayzyas 0:f4e2a62331ef 140 led_ind2 = 0;
sayzyas 0:f4e2a62331ef 141 led_ind3 = 0;
sayzyas 0:f4e2a62331ef 142 led_ind4 = 0;
sayzyas 0:f4e2a62331ef 143 led_ind5 = 0;
sayzyas 0:f4e2a62331ef 144 led_ind6 = 0;
sayzyas 0:f4e2a62331ef 145 led_ind7 = 0;
sayzyas 0:f4e2a62331ef 146 led1 = 0; // off
sayzyas 0:f4e2a62331ef 147 led2 = 1; // on
sayzyas 0:f4e2a62331ef 148 led3 = 0; // off
sayzyas 0:f4e2a62331ef 149 led4 = 0; // off
sayzyas 2:d0a5ee4e7bb8 150 Thread::wait(15);
sayzyas 0:f4e2a62331ef 151 led_ind0 = 1;
sayzyas 0:f4e2a62331ef 152 led_ind1 = 1;
sayzyas 0:f4e2a62331ef 153 led_ind2 = 1;
sayzyas 0:f4e2a62331ef 154 led_ind3 = 1;
sayzyas 0:f4e2a62331ef 155 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 156 led_ind5 = 1;
sayzyas 0:f4e2a62331ef 157 led_ind6 = 1;
sayzyas 0:f4e2a62331ef 158 led_ind7 = 1;
sayzyas 0:f4e2a62331ef 159 led1 = 0; // off
sayzyas 0:f4e2a62331ef 160 led2 = 0; // off
sayzyas 0:f4e2a62331ef 161 led3 = 1; // on
sayzyas 0:f4e2a62331ef 162 led4 = 0; // off
sayzyas 2:d0a5ee4e7bb8 163 Thread::wait(15);
sayzyas 0:f4e2a62331ef 164 led_ind0 = 0;
sayzyas 0:f4e2a62331ef 165 led_ind1 = 0;
sayzyas 0:f4e2a62331ef 166 led_ind2 = 0;
sayzyas 0:f4e2a62331ef 167 led_ind3 = 0;
sayzyas 0:f4e2a62331ef 168 led_ind4 = 0;
sayzyas 0:f4e2a62331ef 169 led_ind5 = 0;
sayzyas 0:f4e2a62331ef 170 led_ind6 = 0;
sayzyas 0:f4e2a62331ef 171 led_ind7 = 0;
sayzyas 0:f4e2a62331ef 172 led1 = 0; // off
sayzyas 0:f4e2a62331ef 173 led2 = 0; // off
sayzyas 0:f4e2a62331ef 174 led3 = 0; // off
sayzyas 0:f4e2a62331ef 175 led4 = 1; // on
sayzyas 2:d0a5ee4e7bb8 176 Thread::wait(15);
sayzyas 0:f4e2a62331ef 177 led_ind0 = 1;
sayzyas 0:f4e2a62331ef 178 led_ind1 = 1;
sayzyas 0:f4e2a62331ef 179 led_ind2 = 1;
sayzyas 0:f4e2a62331ef 180 led_ind3 = 1;
sayzyas 0:f4e2a62331ef 181 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 182 led_ind5 = 1;
sayzyas 0:f4e2a62331ef 183 led_ind6 = 1;
sayzyas 0:f4e2a62331ef 184 led_ind7 = 1;
sayzyas 0:f4e2a62331ef 185 led1 = 0; // off
sayzyas 0:f4e2a62331ef 186 led2 = 0; // off
sayzyas 0:f4e2a62331ef 187 led3 = 0; // off
sayzyas 0:f4e2a62331ef 188 led4 = 0; // off
sayzyas 0:f4e2a62331ef 189 }
sayzyas 0:f4e2a62331ef 190 }
sayzyas 0:f4e2a62331ef 191
sayzyas 0:f4e2a62331ef 192 // *****************************************************************
sayzyas 0:f4e2a62331ef 193 // calibration joystick data
sayzyas 0:f4e2a62331ef 194 // *****************************************************************
sayzyas 0:f4e2a62331ef 195 void calibrate_joystick( int mode, uint8_t data_ud, uint8_t data_lr ){
sayzyas 0:f4e2a62331ef 196 uint16_t tmp;
sayzyas 0:f4e2a62331ef 197
sayzyas 0:f4e2a62331ef 198 if( mode == 0 )
sayzyas 0:f4e2a62331ef 199 {
sayzyas 0:f4e2a62331ef 200 tmp = (uint16_t)js_center_value_r_ud;
sayzyas 0:f4e2a62331ef 201 tmp += (uint16_t)data_ud;
sayzyas 0:f4e2a62331ef 202 tmp /= 2;
sayzyas 0:f4e2a62331ef 203 js_center_value_r_ud = (uint8_t)tmp;
sayzyas 0:f4e2a62331ef 204
sayzyas 0:f4e2a62331ef 205 tmp = (uint16_t)js_center_value_r_lr;
sayzyas 0:f4e2a62331ef 206 tmp += (uint16_t)data_lr;
sayzyas 0:f4e2a62331ef 207 tmp /= 2;
sayzyas 0:f4e2a62331ef 208 js_center_value_r_lr = (uint8_t)tmp;
sayzyas 0:f4e2a62331ef 209 }
sayzyas 0:f4e2a62331ef 210 else
sayzyas 0:f4e2a62331ef 211 {
sayzyas 0:f4e2a62331ef 212
sayzyas 0:f4e2a62331ef 213 tmp = (uint16_t)js_center_value_l_ud;
sayzyas 0:f4e2a62331ef 214 tmp += (uint16_t)data_ud;
sayzyas 0:f4e2a62331ef 215 tmp /= 2;
sayzyas 0:f4e2a62331ef 216 js_center_value_l_ud = (uint8_t)tmp;
sayzyas 0:f4e2a62331ef 217
sayzyas 0:f4e2a62331ef 218 tmp = (uint16_t)js_center_value_l_lr;
sayzyas 0:f4e2a62331ef 219 tmp += (uint16_t)data_lr;
sayzyas 0:f4e2a62331ef 220 tmp /= 2;
sayzyas 0:f4e2a62331ef 221 js_center_value_l_lr = (uint8_t)tmp;
sayzyas 0:f4e2a62331ef 222 }
sayzyas 0:f4e2a62331ef 223 }
sayzyas 0:f4e2a62331ef 224
sayzyas 0:f4e2a62331ef 225 // *****************************************************************
sayzyas 0:f4e2a62331ef 226 // calcuilation motor speed
sayzyas 0:f4e2a62331ef 227 // *****************************************************************
sayzyas 0:f4e2a62331ef 228 uint8_t calc_motor_speed( uint8_t data, int32_t dir ){
sayzyas 0:f4e2a62331ef 229 uint8_t speed;
sayzyas 0:f4e2a62331ef 230 if( dir == 0 ){
sayzyas 0:f4e2a62331ef 231 // speed = (data - js_center_value) * 100 / js_center_value;
sayzyas 0:f4e2a62331ef 232 if( speed > 95 ) speed = 100;
sayzyas 0:f4e2a62331ef 233 }
sayzyas 0:f4e2a62331ef 234 else{
sayzyas 0:f4e2a62331ef 235 // speed = (js_center_value - data) * 100 / js_center_value;
sayzyas 0:f4e2a62331ef 236 if( speed > 95 ) speed = 100;
sayzyas 0:f4e2a62331ef 237 }
sayzyas 0:f4e2a62331ef 238 return speed;
sayzyas 0:f4e2a62331ef 239 }
sayzyas 0:f4e2a62331ef 240
sayzyas 0:f4e2a62331ef 241 // *****************************************************************
sayzyas 0:f4e2a62331ef 242 // switch control task
sayzyas 0:f4e2a62331ef 243 // ----------------------
sayzyas 0:f4e2a62331ef 244 //
sayzyas 0:f4e2a62331ef 245 // Up: center -> 255
sayzyas 0:f4e2a62331ef 246 // Down: 002 -> center
sayzyas 0:f4e2a62331ef 247 // Right: center -> 255
sayzyas 0:f4e2a62331ef 248 // Left : 0 -> center
sayzyas 0:f4e2a62331ef 249 //
sayzyas 0:f4e2a62331ef 250 // *****************************************************************
sayzyas 0:f4e2a62331ef 251 void sw_task( void const *){
sayzyas 0:f4e2a62331ef 252
sayzyas 2:d0a5ee4e7bb8 253 char msg[16];
sayzyas 0:f4e2a62331ef 254
sayzyas 0:f4e2a62331ef 255 uint16_t js_rud_data;
sayzyas 0:f4e2a62331ef 256 uint16_t js_rlr_data;
sayzyas 0:f4e2a62331ef 257 uint8_t js_rud_undata;
sayzyas 0:f4e2a62331ef 258 uint8_t js_rlr_undata;
sayzyas 0:f4e2a62331ef 259
sayzyas 0:f4e2a62331ef 260 uint16_t js_lud_data;
sayzyas 0:f4e2a62331ef 261 uint16_t js_llr_data;
sayzyas 0:f4e2a62331ef 262 uint8_t js_lud_undata;
sayzyas 0:f4e2a62331ef 263 uint8_t js_llr_undata;
sayzyas 0:f4e2a62331ef 264
sayzyas 0:f4e2a62331ef 265 float motor_speed_f = 0;
sayzyas 0:f4e2a62331ef 266 uint8_t motor_speed = 0;
sayzyas 0:f4e2a62331ef 267
sayzyas 0:f4e2a62331ef 268 send_report.length = 8;
sayzyas 0:f4e2a62331ef 269
sayzyas 1:368ba89c2e6b 270 send_report.data[0] = 0x80; // L JoyStick Left/Right
sayzyas 0:f4e2a62331ef 271 send_report.data[1] = 0x80; // L JoyStick Up/Down
sayzyas 1:368ba89c2e6b 272 send_report.data[2] = 0x80; // R JoyStick Left/Right^
sayzyas 0:f4e2a62331ef 273 send_report.data[3] = 0x80; // R JoyStick Up/Down
sayzyas 0:f4e2a62331ef 274 send_report.data[4] = 0x00; // Operatiuon Switch
sayzyas 1:368ba89c2e6b 275 // Data Bytes
sayzyas 0:f4e2a62331ef 276 // 7 6 5 4 3 2 1 0
sayzyas 1:368ba89c2e6b 277 // +-+-+-+-+-+----+----+----+----+
sayzyas 1:368ba89c2e6b 278 // |-|-|-|-|-|LB-I|LB-K|RF-I|RF-K|
sayzyas 1:368ba89c2e6b 279 // +-+-+-+-+-+----+----+----+----+
sayzyas 0:f4e2a62331ef 280
sayzyas 0:f4e2a62331ef 281 send_report.data[5] = 0x00; // JS mode switch
sayzyas 1:368ba89c2e6b 282 // Command Bit
sayzyas 0:f4e2a62331ef 283 // 7 6 5 4 3 2 1 0
sayzyas 0:f4e2a62331ef 284 // +-+-+-+-+-+-+---+----+
sayzyas 0:f4e2a62331ef 285 // |-|-|-|-|-|-|S/D|I/KO|
sayzyas 0:f4e2a62331ef 286 // +-+-+-+-+-+-+---+----+
sayzyas 0:f4e2a62331ef 287 // 1:S 1:KO <--- sw off
sayzyas 0:f4e2a62331ef 288 // 0:D 0:I <--- sw on
sayzyas 0:f4e2a62331ef 289
sayzyas 0:f4e2a62331ef 290
sayzyas 0:f4e2a62331ef 291
sayzyas 0:f4e2a62331ef 292 send_report.data[6] = 0x00; // No Function
sayzyas 0:f4e2a62331ef 293 send_report.data[7] = 0x00; // No Function
sayzyas 0:f4e2a62331ef 294 send_report.data[8] = 0x00;
sayzyas 0:f4e2a62331ef 295
sayzyas 0:f4e2a62331ef 296 DEBUG_PRINT_L1("Calibrating joystick ... ");
sayzyas 2:d0a5ee4e7bb8 297 // for( int i = 0; i < CALIBRATION_COUNT; i++){
sayzyas 2:d0a5ee4e7bb8 298 for( int i = 0; i < 10; i++){
sayzyas 0:f4e2a62331ef 299 js_rud_data = js_r_ud.read_u16();
sayzyas 0:f4e2a62331ef 300 js_rlr_data = js_r_lr.read_u16();
sayzyas 0:f4e2a62331ef 301 js_rud_undata = (uint8_t)(js_rud_data >> 8);
sayzyas 0:f4e2a62331ef 302 js_rlr_undata = (uint8_t)(js_rlr_data >> 8);
sayzyas 0:f4e2a62331ef 303 calibrate_joystick( 0, js_rud_undata, js_rlr_undata); // Under constraction
sayzyas 0:f4e2a62331ef 304 js_lud_data = js_l_ud.read_u16();
sayzyas 0:f4e2a62331ef 305 js_llr_data = js_l_lr.read_u16();
sayzyas 0:f4e2a62331ef 306 js_lud_undata = (uint8_t)(js_lud_data >> 8);
sayzyas 0:f4e2a62331ef 307 js_llr_undata = (uint8_t)(js_llr_data >> 8);
sayzyas 0:f4e2a62331ef 308 calibrate_joystick( 1, js_lud_undata, js_llr_undata); // Under constraction
sayzyas 0:f4e2a62331ef 309 }
sayzyas 0:f4e2a62331ef 310 DEBUG_PRINT_L1("done\r\n");
sayzyas 0:f4e2a62331ef 311
sayzyas 0:f4e2a62331ef 312 while(1){
sayzyas 0:f4e2a62331ef 313 js_rud_data = js_r_ud.read_u16();
sayzyas 0:f4e2a62331ef 314 js_rlr_data = js_r_lr.read_u16();
sayzyas 0:f4e2a62331ef 315 js_lud_data = js_l_ud.read_u16();
sayzyas 0:f4e2a62331ef 316 js_llr_data = js_l_lr.read_u16();
sayzyas 0:f4e2a62331ef 317
sayzyas 0:f4e2a62331ef 318 js_rud_undata = (uint8_t)(js_rud_data >> 8);
sayzyas 0:f4e2a62331ef 319 js_rlr_undata = (uint8_t)(js_rlr_data >> 8);
sayzyas 0:f4e2a62331ef 320 js_lud_undata = (uint8_t)(js_lud_data >> 8);
sayzyas 0:f4e2a62331ef 321 js_llr_undata = (uint8_t)(js_llr_data >> 8);
sayzyas 0:f4e2a62331ef 322
sayzyas 0:f4e2a62331ef 323 DEBUG_PRINT_L1("Handy> ");
sayzyas 0:f4e2a62331ef 324 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 325 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 326 DEBUG_PRINT_L1("SW: %d %d %d ", sw1.read(), sw2.read(), sw3.read() );
sayzyas 2:d0a5ee4e7bb8 327 DEBUG_PRINT_L1("JS: %d %d\r\n", sw_ikmode.read(), sw_validpart.read() );
sayzyas 0:f4e2a62331ef 328
sayzyas 0:f4e2a62331ef 329 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 330 // Right JoyStick Control
sayzyas 0:f4e2a62331ef 331 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 332 // Down
sayzyas 2:d0a5ee4e7bb8 333 // if( js_rud_undata > (js_center_value_r_ud + DEAD_ZONE_BAND_WIDTH) ){
sayzyas 2:d0a5ee4e7bb8 334 if( js_rud_undata > (js_center_value_r_ud + 5) ){
sayzyas 0:f4e2a62331ef 335 // Crawler Forward Run
sayzyas 0:f4e2a62331ef 336 led4 = 1;
sayzyas 0:f4e2a62331ef 337 motor_speed = calc_motor_speed( js_rud_undata, 0 );
sayzyas 0:f4e2a62331ef 338 send_report.data[3] = 255 - js_rud_undata; // speed
sayzyas 2:d0a5ee4e7bb8 339 DEBUG_PRINT_L2("Handy> R-JS Up: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rud_undata, send_report.data[3], js_center_value_r_ud);
sayzyas 2:d0a5ee4e7bb8 340 if ( sw_validpart == 0 ){
sayzyas 2:d0a5ee4e7bb8 341 //------------123 45 --------------------
sayzyas 2:d0a5ee4e7bb8 342 sprintf(msg, "F%03d", send_report.data[3]);
sayzyas 2:d0a5ee4e7bb8 343 lcd_out2(6,1,msg,4);
sayzyas 2:d0a5ee4e7bb8 344 }
sayzyas 0:f4e2a62331ef 345 led4 = 0;
sayzyas 0:f4e2a62331ef 346 }
sayzyas 0:f4e2a62331ef 347 // Up
sayzyas 2:d0a5ee4e7bb8 348 // else if( js_rud_undata < (js_center_value_r_ud - DEAD_ZONE_BAND_WIDTH) ){
sayzyas 2:d0a5ee4e7bb8 349 else if( js_rud_undata < (js_center_value_r_ud - 5) ){
sayzyas 0:f4e2a62331ef 350 // Crawler Reverse Run
sayzyas 0:f4e2a62331ef 351 led4 = 1;
sayzyas 0:f4e2a62331ef 352 motor_speed = calc_motor_speed( js_rud_undata, 1 );
sayzyas 0:f4e2a62331ef 353 send_report.data[3] = 255 - js_rud_undata; // speed
sayzyas 2:d0a5ee4e7bb8 354 DEBUG_PRINT_L2("Handy> R-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rud_undata, send_report.data[3], js_center_value_r_ud);
sayzyas 2:d0a5ee4e7bb8 355 if ( sw_validpart == 0 ){
sayzyas 2:d0a5ee4e7bb8 356 //-----------123456789 123456--------------------
sayzyas 2:d0a5ee4e7bb8 357 sprintf(msg, "B%03d", send_report.data[3]);
sayzyas 2:d0a5ee4e7bb8 358 lcd_out2(6,1,msg,4);
sayzyas 2:d0a5ee4e7bb8 359 }
sayzyas 0:f4e2a62331ef 360 led4 = 0;
sayzyas 0:f4e2a62331ef 361 }
sayzyas 0:f4e2a62331ef 362 else{
sayzyas 0:f4e2a62331ef 363 send_report.data[3] = 0x80; // speed
sayzyas 2:d0a5ee4e7bb8 364 // calibrate_joystick(js_ud_un); // Under constraction
sayzyas 0:f4e2a62331ef 365 led4 = 0;
sayzyas 0:f4e2a62331ef 366 }
sayzyas 0:f4e2a62331ef 367
sayzyas 1:368ba89c2e6b 368 // Right
sayzyas 2:d0a5ee4e7bb8 369 // if( js_rlr_undata > (js_center_value_r_ud + DEAD_ZONE_BAND_WIDTH) ){
sayzyas 2:d0a5ee4e7bb8 370 if( js_rlr_undata > (js_center_value_r_ud + 5) ){
sayzyas 0:f4e2a62331ef 371 // Crawler Forward Run
sayzyas 0:f4e2a62331ef 372 led4 = 1;
sayzyas 0:f4e2a62331ef 373 motor_speed = calc_motor_speed( js_rlr_undata, 0 );
sayzyas 0:f4e2a62331ef 374 send_report.data[2] = js_rlr_undata; // speed
sayzyas 2:d0a5ee4e7bb8 375 DEBUG_PRINT_L2("Handy> R-JS Right: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rlr_undata, send_report.data[2], js_center_value_r_ud);
sayzyas 2:d0a5ee4e7bb8 376 if ( sw_validpart == 0 ){
sayzyas 2:d0a5ee4e7bb8 377 //-----------123456789 123456--------------------
sayzyas 2:d0a5ee4e7bb8 378 sprintf(msg,"R%03d", send_report.data[2]);
sayzyas 2:d0a5ee4e7bb8 379 lcd_out2(6,1,msg,4);
sayzyas 2:d0a5ee4e7bb8 380 }
sayzyas 0:f4e2a62331ef 381 led4 = 0;
sayzyas 0:f4e2a62331ef 382 }
sayzyas 0:f4e2a62331ef 383
sayzyas 0:f4e2a62331ef 384 // Left
sayzyas 2:d0a5ee4e7bb8 385 // else if( js_rlr_undata < (js_center_value_r_ud - DEAD_ZONE_BAND_WIDTH) ){
sayzyas 2:d0a5ee4e7bb8 386 else if( js_rlr_undata < (js_center_value_r_ud - 5) ){
sayzyas 0:f4e2a62331ef 387 // Crawler Reverse Run
sayzyas 0:f4e2a62331ef 388 led4 = 1;
sayzyas 0:f4e2a62331ef 389 motor_speed = calc_motor_speed( js_rlr_undata, 1 );
sayzyas 0:f4e2a62331ef 390 send_report.data[2] = js_rlr_undata; // speed
sayzyas 2:d0a5ee4e7bb8 391 DEBUG_PRINT_L2("Handy> R-JS Left: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_rlr_undata, send_report.data[2], js_center_value_r_ud);
sayzyas 2:d0a5ee4e7bb8 392 if ( sw_validpart == 0 ){
sayzyas 2:d0a5ee4e7bb8 393 //-----------123456789 123456--------------------
sayzyas 2:d0a5ee4e7bb8 394 sprintf(msg,"L%03d", send_report.data[2]);
sayzyas 2:d0a5ee4e7bb8 395 lcd_out2(6,1,msg,4);
sayzyas 2:d0a5ee4e7bb8 396 }
sayzyas 0:f4e2a62331ef 397 led4 = 0;
sayzyas 0:f4e2a62331ef 398 }
sayzyas 0:f4e2a62331ef 399
sayzyas 0:f4e2a62331ef 400 else{
sayzyas 0:f4e2a62331ef 401 send_report.data[2] = 0x80; // speed
sayzyas 0:f4e2a62331ef 402 // calibrate_joystick(js_ud_un); // Under constraction
sayzyas 0:f4e2a62331ef 403 led4 = 0;
sayzyas 0:f4e2a62331ef 404 }
sayzyas 0:f4e2a62331ef 405
sayzyas 2:d0a5ee4e7bb8 406 if( ( send_report.data[2] == 0x80 ) && ( send_report.data[3] == 0x80 ) ){
sayzyas 2:d0a5ee4e7bb8 407 lcd_out2(6,1," ",4);
sayzyas 2:d0a5ee4e7bb8 408 }
sayzyas 2:d0a5ee4e7bb8 409
sayzyas 0:f4e2a62331ef 410 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 411 // Left JoyStick Control
sayzyas 0:f4e2a62331ef 412 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 413 // Down
sayzyas 2:d0a5ee4e7bb8 414 // if( js_lud_undata > (js_center_value_l_ud + DEAD_ZONE_BAND_WIDTH) ){
sayzyas 2:d0a5ee4e7bb8 415 if( js_lud_undata > (js_center_value_l_ud + 5) ){
sayzyas 0:f4e2a62331ef 416 // Crawler Forward Run
sayzyas 0:f4e2a62331ef 417 led4 = 1;
sayzyas 0:f4e2a62331ef 418 motor_speed = calc_motor_speed( js_lud_undata, 0 );
sayzyas 0:f4e2a62331ef 419 send_report.data[1] = 255 - js_lud_undata; // speed
sayzyas 2:d0a5ee4e7bb8 420 DEBUG_PRINT_L2("Handy> L-JS Up: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_lud_undata, send_report.data[1], js_center_value_l_ud);
sayzyas 2:d0a5ee4e7bb8 421 if ( sw_validpart == 0 ){
sayzyas 2:d0a5ee4e7bb8 422 //-----------123456789 123456--------------------
sayzyas 2:d0a5ee4e7bb8 423 sprintf(msg,"F%03d", send_report.data[1]);
sayzyas 2:d0a5ee4e7bb8 424 lcd_out2(1,1,msg,4);
sayzyas 2:d0a5ee4e7bb8 425 }
sayzyas 0:f4e2a62331ef 426 led4 = 0;
sayzyas 0:f4e2a62331ef 427 }
sayzyas 0:f4e2a62331ef 428 // Up
sayzyas 2:d0a5ee4e7bb8 429 // else if( js_lud_undata < (js_center_value_l_ud - DEAD_ZONE_BAND_WIDTH) ){
sayzyas 2:d0a5ee4e7bb8 430 else if( js_lud_undata < (js_center_value_l_ud - 5) ){
sayzyas 0:f4e2a62331ef 431 // Crawler Reverse Run
sayzyas 0:f4e2a62331ef 432 led4 = 1;
sayzyas 0:f4e2a62331ef 433 motor_speed = calc_motor_speed( js_lud_undata, 1 );
sayzyas 0:f4e2a62331ef 434 send_report.data[1] = 255 - js_lud_undata; // speed
sayzyas 2:d0a5ee4e7bb8 435 DEBUG_PRINT_L2("Handy> L-JS Down: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_lud_undata, send_report.data[1], js_center_value_l_ud);
sayzyas 2:d0a5ee4e7bb8 436 if ( sw_validpart == 0 ){
sayzyas 2:d0a5ee4e7bb8 437 //-----------123456789 123456--------------------
sayzyas 2:d0a5ee4e7bb8 438 sprintf(msg,"B%03d", send_report.data[1]);
sayzyas 2:d0a5ee4e7bb8 439 lcd_out2(1,1,msg,4);
sayzyas 2:d0a5ee4e7bb8 440 }
sayzyas 0:f4e2a62331ef 441 led4 = 0;
sayzyas 0:f4e2a62331ef 442 }
sayzyas 0:f4e2a62331ef 443 else{
sayzyas 0:f4e2a62331ef 444 send_report.data[1] = 0x80; // speed
sayzyas 0:f4e2a62331ef 445 // calibrate_joystick(js_l_ud_un); // Under constraction
sayzyas 0:f4e2a62331ef 446 led4 = 0;
sayzyas 0:f4e2a62331ef 447 }
sayzyas 0:f4e2a62331ef 448
sayzyas 0:f4e2a62331ef 449 // Right
sayzyas 2:d0a5ee4e7bb8 450 // if( js_llr_undata > (js_center_value_l_ud + DEAD_ZONE_BAND_WIDTH) ){
sayzyas 2:d0a5ee4e7bb8 451 if( js_llr_undata > (js_center_value_l_ud + 5) ){
sayzyas 0:f4e2a62331ef 452 // Crawler Forward Run
sayzyas 0:f4e2a62331ef 453 led4 = 1;
sayzyas 0:f4e2a62331ef 454 motor_speed = calc_motor_speed( js_llr_undata, 0 );
sayzyas 0:f4e2a62331ef 455 send_report.data[0] = js_llr_undata; // speed
sayzyas 2:d0a5ee4e7bb8 456 DEBUG_PRINT_L2("Handy> L-JS Right: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_llr_undata, send_report.data[0], js_center_value_l_ud);
sayzyas 2:d0a5ee4e7bb8 457 if ( sw_validpart == 0 ){
sayzyas 2:d0a5ee4e7bb8 458 //-----------123456789 123456--------------------
sayzyas 2:d0a5ee4e7bb8 459 sprintf(msg,"R%03d", send_report.data[0]);
sayzyas 2:d0a5ee4e7bb8 460 lcd_out2(1,1,msg,4);
sayzyas 2:d0a5ee4e7bb8 461 }
sayzyas 0:f4e2a62331ef 462 led4 = 0;
sayzyas 0:f4e2a62331ef 463 }
sayzyas 0:f4e2a62331ef 464 // Left
sayzyas 2:d0a5ee4e7bb8 465 // else if( js_llr_undata < (js_center_value_l_ud - DEAD_ZONE_BAND_WIDTH) ){
sayzyas 2:d0a5ee4e7bb8 466 else if( js_llr_undata < (js_center_value_l_ud - 5) ){
sayzyas 0:f4e2a62331ef 467 // Crawler Reverse Run
sayzyas 0:f4e2a62331ef 468 led4 = 1;
sayzyas 0:f4e2a62331ef 469 motor_speed = calc_motor_speed( js_llr_undata, 1 );
sayzyas 0:f4e2a62331ef 470 send_report.data[0] = js_llr_undata; // speed
sayzyas 2:d0a5ee4e7bb8 471 DEBUG_PRINT_L2("Handy> L-JS Left: js_data[%03d], motor_speeed[%03d], js_center[%03d]\r\n", js_llr_undata, send_report.data[0], js_center_value_l_ud);
sayzyas 2:d0a5ee4e7bb8 472 if ( sw_validpart == 0 ){
sayzyas 2:d0a5ee4e7bb8 473 //-----------123456789 123456--------------------
sayzyas 2:d0a5ee4e7bb8 474 sprintf(msg,"L%03d", send_report.data[0]);
sayzyas 2:d0a5ee4e7bb8 475 lcd_out2(1,1,msg,4);
sayzyas 2:d0a5ee4e7bb8 476 }
sayzyas 0:f4e2a62331ef 477 led4 = 0;
sayzyas 0:f4e2a62331ef 478 }
sayzyas 0:f4e2a62331ef 479
sayzyas 0:f4e2a62331ef 480 else{
sayzyas 0:f4e2a62331ef 481 send_report.data[0] = 0x80; // speed
sayzyas 0:f4e2a62331ef 482 // calibrate_joystick(js_l_ud_un); // Under constraction
sayzyas 0:f4e2a62331ef 483 led4 = 0;
sayzyas 0:f4e2a62331ef 484 }
sayzyas 2:d0a5ee4e7bb8 485
sayzyas 2:d0a5ee4e7bb8 486 if( ( send_report.data[0] == 0x80 ) && ( send_report.data[1] == 0x80 ) ){
sayzyas 2:d0a5ee4e7bb8 487 lcd_out2(1,1," ",4);
sayzyas 2:d0a5ee4e7bb8 488 }
sayzyas 0:f4e2a62331ef 489
sayzyas 0:f4e2a62331ef 490 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 491 // Other Switch Control: Pass
sayzyas 0:f4e2a62331ef 492 // ---------------------------------------------
sayzyas 0:f4e2a62331ef 493 if ((sw1 == 0)&&(sw2 == 1)&&(sw3==1)){
sayzyas 0:f4e2a62331ef 494 led3 = 1;
sayzyas 0:f4e2a62331ef 495 send_report.data[4] = 0x01; // RF Tfm K (Fwd)
sayzyas 0:f4e2a62331ef 496 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 497 led_ind5 = 1;
sayzyas 1:368ba89c2e6b 498 led_ind6 = 1;
sayzyas 2:d0a5ee4e7bb8 499 led_ind7 = 1;
sayzyas 2:d0a5ee4e7bb8 500 // 123456
sayzyas 2:d0a5ee4e7bb8 501 if ( sw_validpart == 0 )
sayzyas 2:d0a5ee4e7bb8 502 // 0123456789012345
sayzyas 2:d0a5ee4e7bb8 503 lcd_out2(10,1,"RF[P] ",6);
sayzyas 0:f4e2a62331ef 504 }
sayzyas 0:f4e2a62331ef 505 else if ((sw1 == 1)&&(sw2 == 0)&&(sw3==1)){
sayzyas 0:f4e2a62331ef 506 send_report.data[4] = 0x02; // RF Tfm I (Rvs)
sayzyas 0:f4e2a62331ef 507 led3 = 1;
sayzyas 0:f4e2a62331ef 508 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 509 led_ind5 = 1;
sayzyas 1:368ba89c2e6b 510 led_ind6 = 1;
sayzyas 2:d0a5ee4e7bb8 511 led_ind7 = 1;
sayzyas 2:d0a5ee4e7bb8 512 // 123456
sayzyas 2:d0a5ee4e7bb8 513 if ( sw_validpart == 0 )
sayzyas 2:d0a5ee4e7bb8 514 // 0123456789012345
sayzyas 2:d0a5ee4e7bb8 515 lcd_out2(10,1,"RF[I] ",6);
sayzyas 0:f4e2a62331ef 516 }
sayzyas 0:f4e2a62331ef 517 else if ((sw1 == 0)&&(sw2 == 0)&&(sw3==1)){
sayzyas 0:f4e2a62331ef 518 send_report.data[4] = 0x04; // LB Tfm K (Fwd)
sayzyas 0:f4e2a62331ef 519 led3 = 1;
sayzyas 0:f4e2a62331ef 520 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 521 led_ind5 = 1;
sayzyas 1:368ba89c2e6b 522 led_ind6 = 1;
sayzyas 2:d0a5ee4e7bb8 523 led_ind7 = 1;
sayzyas 2:d0a5ee4e7bb8 524 // 123456
sayzyas 2:d0a5ee4e7bb8 525 if ( sw_validpart == 0 )
sayzyas 2:d0a5ee4e7bb8 526 // 0123456789012345
sayzyas 2:d0a5ee4e7bb8 527 lcd_out2(10,1,"LB[P] ",6);
sayzyas 0:f4e2a62331ef 528 }
sayzyas 0:f4e2a62331ef 529 else if ((sw1 == 1)&&(sw2 == 1)&&(sw3==0)){
sayzyas 0:f4e2a62331ef 530 led3 = 1;
sayzyas 0:f4e2a62331ef 531 send_report.data[4] = 0x08; // LB Tfm I (Rvs)
sayzyas 0:f4e2a62331ef 532 led_ind4 = 1;
sayzyas 0:f4e2a62331ef 533 led_ind5 = 1;
sayzyas 1:368ba89c2e6b 534 led_ind6 = 1;
sayzyas 2:d0a5ee4e7bb8 535 led_ind7 = 1;
sayzyas 2:d0a5ee4e7bb8 536 // 123456
sayzyas 2:d0a5ee4e7bb8 537 if ( sw_validpart == 0 )
sayzyas 2:d0a5ee4e7bb8 538 // 0123456789012345
sayzyas 2:d0a5ee4e7bb8 539 lcd_out2(10,1,"LB[I] ",6);
sayzyas 0:f4e2a62331ef 540 }
sayzyas 0:f4e2a62331ef 541 else if ((sw1 == 1)&&(sw2 == 0)&&(sw3==0)){
sayzyas 0:f4e2a62331ef 542 led2 = 1;
sayzyas 0:f4e2a62331ef 543 send_report.data[4] = 0x10; // Winch Down (Fwd)
sayzyas 0:f4e2a62331ef 544 led_ind0 = 1;
sayzyas 0:f4e2a62331ef 545 led_ind1 = 1;
sayzyas 1:368ba89c2e6b 546 led_ind2 = 1;
sayzyas 2:d0a5ee4e7bb8 547 led_ind3 = 1;
sayzyas 2:d0a5ee4e7bb8 548 // 123456
sayzyas 2:d0a5ee4e7bb8 549 if ( sw_validpart == 1 )
sayzyas 2:d0a5ee4e7bb8 550 // 0123456789012345
sayzyas 2:d0a5ee4e7bb8 551 lcd_out2(10,1,"Wch Dn",6);
sayzyas 0:f4e2a62331ef 552 }
sayzyas 0:f4e2a62331ef 553 else if ((sw1 == 0)&&(sw2 == 1)&&(sw3==0)){
sayzyas 0:f4e2a62331ef 554 led2 = 1;
sayzyas 0:f4e2a62331ef 555 send_report.data[4] = 0x20; // Winch Up (Rvs)
sayzyas 0:f4e2a62331ef 556 led_ind0 = 1;
sayzyas 0:f4e2a62331ef 557 led_ind1 = 1;
sayzyas 1:368ba89c2e6b 558 led_ind2 = 1;
sayzyas 2:d0a5ee4e7bb8 559 led_ind3 = 1;
sayzyas 2:d0a5ee4e7bb8 560 // 123456
sayzyas 2:d0a5ee4e7bb8 561 if ( sw_validpart == 1 )
sayzyas 2:d0a5ee4e7bb8 562 // 0123456789012345
sayzyas 2:d0a5ee4e7bb8 563 lcd_out2(10,1,"Wch Up",6);
sayzyas 0:f4e2a62331ef 564 }
sayzyas 0:f4e2a62331ef 565 else{
sayzyas 0:f4e2a62331ef 566 send_report.data[4] = 0;
sayzyas 2:d0a5ee4e7bb8 567 /*
sayzyas 0:f4e2a62331ef 568 led_ind0 = 0;
sayzyas 0:f4e2a62331ef 569 led_ind1 = 0;
sayzyas 0:f4e2a62331ef 570 led_ind2 = 0;
sayzyas 0:f4e2a62331ef 571 led_ind3 = 0;
sayzyas 0:f4e2a62331ef 572 led_ind4 = 0;
sayzyas 0:f4e2a62331ef 573 led_ind5 = 0;
sayzyas 0:f4e2a62331ef 574 led_ind6 = 0;
sayzyas 0:f4e2a62331ef 575 led_ind7 = 0;
sayzyas 2:d0a5ee4e7bb8 576 */
sayzyas 2:d0a5ee4e7bb8 577 led2 = 0;
sayzyas 0:f4e2a62331ef 578 led3 = 0;
sayzyas 2:d0a5ee4e7bb8 579 // 0123456789012345
sayzyas 2:d0a5ee4e7bb8 580 lcd_out2(10,1," ",6);
sayzyas 0:f4e2a62331ef 581 }
sayzyas 1:368ba89c2e6b 582 // Command Bit
sayzyas 0:f4e2a62331ef 583 // 7 6 5 4 3 2 1 0
sayzyas 0:f4e2a62331ef 584 // +-+-+-+-+-+-+---+----+
sayzyas 0:f4e2a62331ef 585 // |-|-|-|-|-|-|S/D|I/KO|
sayzyas 0:f4e2a62331ef 586 // +-+-+-+-+-+-+---+----+
sayzyas 0:f4e2a62331ef 587 // 1:S 1:KO <--- sw off
sayzyas 0:f4e2a62331ef 588 // 0:D 0:I <--- sw on
sayzyas 2:d0a5ee4e7bb8 589 if( sw_validpart == 1 ){ // sw off
sayzyas 2:d0a5ee4e7bb8 590 send_report.data[5] |= 0x02; // SW off: Winch valid
sayzyas 0:f4e2a62331ef 591 }
sayzyas 0:f4e2a62331ef 592 else{ // sw off
sayzyas 2:d0a5ee4e7bb8 593 send_report.data[5] &= 0xFD; // SW on: Tfm. crawler valid
sayzyas 0:f4e2a62331ef 594 }
sayzyas 2:d0a5ee4e7bb8 595 if( sw_ikmode == 1 ){ // sw off
sayzyas 2:d0a5ee4e7bb8 596 send_report.data[5] |= 0x01; // off: K mode
sayzyas 0:f4e2a62331ef 597 }
sayzyas 0:f4e2a62331ef 598 else{ // sw off
sayzyas 2:d0a5ee4e7bb8 599 send_report.data[5] &= 0xFE; // off: I mode
sayzyas 0:f4e2a62331ef 600 }
sayzyas 0:f4e2a62331ef 601
sayzyas 0:f4e2a62331ef 602 // *************************
sayzyas 0:f4e2a62331ef 603 // Send Command to HID host: Hot Plug !!
sayzyas 0:f4e2a62331ef 604 // *************************
sayzyas 0:f4e2a62331ef 605 hid.send(&send_report); // send command to USB HID host
sayzyas 0:f4e2a62331ef 606
sayzyas 0:f4e2a62331ef 607 Thread::wait(5);
sayzyas 0:f4e2a62331ef 608 }
sayzyas 0:f4e2a62331ef 609
sayzyas 0:f4e2a62331ef 610 }
sayzyas 0:f4e2a62331ef 611
sayzyas 0:f4e2a62331ef 612 void lcd_out( int column, int row, char* msg ){
sayzyas 0:f4e2a62331ef 613
sayzyas 0:f4e2a62331ef 614 // lcd.cls();
sayzyas 0:f4e2a62331ef 615 lcd.locate(column,row);
sayzyas 0:f4e2a62331ef 616
sayzyas 0:f4e2a62331ef 617 /*
sayzyas 0:f4e2a62331ef 618 for( int i = 0; i < 16; i++ ){
sayzyas 0:f4e2a62331ef 619 lcd.putc(*msg++);
sayzyas 0:f4e2a62331ef 620 }
sayzyas 0:f4e2a62331ef 621 */
sayzyas 0:f4e2a62331ef 622 lcd.printf( msg );
sayzyas 0:f4e2a62331ef 623 Thread::wait(10);
sayzyas 0:f4e2a62331ef 624 }
sayzyas 0:f4e2a62331ef 625
sayzyas 2:d0a5ee4e7bb8 626 void lcd_out2( int column, int row, char* msg, int cnt){
sayzyas 2:d0a5ee4e7bb8 627
sayzyas 2:d0a5ee4e7bb8 628 // lcd.cls();
sayzyas 2:d0a5ee4e7bb8 629
sayzyas 2:d0a5ee4e7bb8 630 for( int i = 0; i < cnt; i++ ){
sayzyas 2:d0a5ee4e7bb8 631 lcd.locate(column+i,row);
sayzyas 2:d0a5ee4e7bb8 632 lcd.putc(*msg++);
sayzyas 2:d0a5ee4e7bb8 633 }
sayzyas 2:d0a5ee4e7bb8 634
sayzyas 2:d0a5ee4e7bb8 635 // lcd.printf( msg );
sayzyas 2:d0a5ee4e7bb8 636 Thread::wait(10);
sayzyas 2:d0a5ee4e7bb8 637 }
sayzyas 0:f4e2a62331ef 638
sayzyas 0:f4e2a62331ef 639 // **********************************************************************
sayzyas 0:f4e2a62331ef 640 //
sayzyas 0:f4e2a62331ef 641 // Main Function of this program
sayzyas 0:f4e2a62331ef 642 //
sayzyas 0:f4e2a62331ef 643 // **********************************************************************
sayzyas 0:f4e2a62331ef 644 int main(void) {
sayzyas 0:f4e2a62331ef 645
sayzyas 0:f4e2a62331ef 646 int32_t counter = 0;
sayzyas 0:f4e2a62331ef 647
sayzyas 0:f4e2a62331ef 648 char msg[16];
sayzyas 0:f4e2a62331ef 649 char buf[11];
sayzyas 0:f4e2a62331ef 650 int i;
sayzyas 0:f4e2a62331ef 651
sayzyas 0:f4e2a62331ef 652 pc.baud(115200);
sayzyas 0:f4e2a62331ef 653
sayzyas 2:d0a5ee4e7bb8 654 sw_ikmode.mode( PullUp ); // use internal pullup
sayzyas 2:d0a5ee4e7bb8 655 sw_validpart.mode( PullUp ); // use internal pullup
sayzyas 2:d0a5ee4e7bb8 656
sayzyas 2:d0a5ee4e7bb8 657 // i2c_saddress = Target_IIC_ADDR; // defined at header
sayzyas 2:d0a5ee4e7bb8 658 i2c_saddress = 0x10; // defined at header
sayzyas 0:f4e2a62331ef 659 i2c_saddress <<= 1;
sayzyas 0:f4e2a62331ef 660 slave.address(i2c_saddress);
sayzyas 0:f4e2a62331ef 661 DEBUG_PRINT_L0("Handy> IIC Address: %02x\r\n",i2c_saddress);
sayzyas 0:f4e2a62331ef 662
sayzyas 2:d0a5ee4e7bb8 663 lcd_out2(0,0,"B2DebrisSurveyor",16);
sayzyas 2:d0a5ee4e7bb8 664 lcd_out2(0,1,"Revast Co.,Ltd. ",16);
sayzyas 0:f4e2a62331ef 665 led_demo();
sayzyas 2:d0a5ee4e7bb8 666 lcd_out2(0,0,"Rev1.00 20160218",16);
sayzyas 2:d0a5ee4e7bb8 667 lcd_out2(0,1,"System Booting..",16);
sayzyas 0:f4e2a62331ef 668
sayzyas 0:f4e2a62331ef 669 DEBUG_PRINT_L0("\r\n");
sayzyas 0:f4e2a62331ef 670 DEBUG_PRINT_L0("Handy> +-----------------------------------------------------------\r\n");
sayzyas 0:f4e2a62331ef 671 DEBUG_PRINT_L0("Handy> | Project: Crawler Explorer for 1F-1 PCV internal inspection\r\n");
sayzyas 0:f4e2a62331ef 672 DEBUG_PRINT_L0("Handy> |---------\r\n");
sayzyas 0:f4e2a62331ef 673 DEBUG_PRINT_L0("Handy> | This is: Handy Controller main program over USB-HID \r\n");
sayzyas 2:d0a5ee4e7bb8 674 // DEBUG_PRINT_L0("Handy> | Target MCU: %s\r\n", Targetmbed);
sayzyas 0:f4e2a62331ef 675 DEBUG_PRINT_L0("Handy> | Letest update: %s\r\n", LatestUpDate);
sayzyas 0:f4e2a62331ef 676 DEBUG_PRINT_L0("Handy> | Program Revision: %s\r\n", ProgramRevision);
sayzyas 0:f4e2a62331ef 677 DEBUG_PRINT_L0("Handy> | Author: %s\r\n", Author);
sayzyas 0:f4e2a62331ef 678 DEBUG_PRINT_L0("Handy> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
sayzyas 0:f4e2a62331ef 679 DEBUG_PRINT_L0("Handy> +-----------------------------------------------------------\r\n");
sayzyas 0:f4e2a62331ef 680
sayzyas 0:f4e2a62331ef 681 Thread task1(sw_task, NULL, osPriorityNormal, 256 * 4);
sayzyas 0:f4e2a62331ef 682
sayzyas 0:f4e2a62331ef 683 // 1234567890123456
sayzyas 2:d0a5ee4e7bb8 684 lcd_out2(0,0,"Booted up ! ",16);
sayzyas 0:f4e2a62331ef 685
sayzyas 0:f4e2a62331ef 686 if(hid.readNB(&recv_report)) {
sayzyas 2:d0a5ee4e7bb8 687 //if(hid.read(&recv_report)) {
sayzyas 0:f4e2a62331ef 688 for(int i = 1; i < recv_report.length; i++) {
sayzyas 0:f4e2a62331ef 689 DEBUG_PRINT_L0("%d ", recv_report.data[i]);
sayzyas 0:f4e2a62331ef 690 }
sayzyas 0:f4e2a62331ef 691 DEBUG_PRINT_L0("\r\n");
sayzyas 0:f4e2a62331ef 692 }
sayzyas 2:d0a5ee4e7bb8 693 // 1234567890123456
sayzyas 2:d0a5ee4e7bb8 694 lcd_out2(0,0,"B2DebrisProbe ",16);
sayzyas 2:d0a5ee4e7bb8 695 Thread::wait(25);
sayzyas 2:d0a5ee4e7bb8 696 lcd_out2(0,0,"System OK[-] ",16);
sayzyas 2:d0a5ee4e7bb8 697 lcd_out2(0,1," ",16);
sayzyas 0:f4e2a62331ef 698
sayzyas 0:f4e2a62331ef 699 while (1) {
sayzyas 0:f4e2a62331ef 700 //try to read a msg
sayzyas 0:f4e2a62331ef 701
sayzyas 0:f4e2a62331ef 702 if(hid.readNB(&recv_report)) {
sayzyas 0:f4e2a62331ef 703 for(int i = 1; i < recv_report.length; i++) {
sayzyas 0:f4e2a62331ef 704 // DEBUG_PRINT("%d ", recv_report.data[i]);
sayzyas 0:f4e2a62331ef 705 // serial.printf("%d ", recv_report.data[i]);
sayzyas 0:f4e2a62331ef 706 }
sayzyas 0:f4e2a62331ef 707 // DEBUG_PRINT("\r\n");
sayzyas 0:f4e2a62331ef 708 // serial.printf("\r\n");
sayzyas 0:f4e2a62331ef 709
sayzyas 0:f4e2a62331ef 710 }
sayzyas 2:d0a5ee4e7bb8 711 // 7 6 5 4 3 2 1 0
sayzyas 2:d0a5ee4e7bb8 712 // +-+-+-+-+-+-+---+----+
sayzyas 2:d0a5ee4e7bb8 713 // |-|-|-|-|-|-|S/D|I/KO|
sayzyas 2:d0a5ee4e7bb8 714 // +-+-+-+-+-+-+---+----+
sayzyas 2:d0a5ee4e7bb8 715 // 1:S 1:KO <--- sw off
sayzyas 2:d0a5ee4e7bb8 716 // 0:D 0:I <--- sw on
sayzyas 2:d0a5ee4e7bb8 717 if (( sw_validpart == 0 )&&( sw_ikmode == 0 )){
sayzyas 2:d0a5ee4e7bb8 718 // 0123456789012345
sayzyas 2:d0a5ee4e7bb8 719 // lcd_out2(0,0," [O][X] | JS I ",16);
sayzyas 2:d0a5ee4e7bb8 720 lcd_out2(0,0,"Crawler Inline ",16);
sayzyas 2:d0a5ee4e7bb8 721 led_crawler_valid(counter);
sayzyas 2:d0a5ee4e7bb8 722 led_ind0 = 0;
sayzyas 2:d0a5ee4e7bb8 723 led_ind1 = 0;
sayzyas 2:d0a5ee4e7bb8 724 led_ind2 = 0;
sayzyas 2:d0a5ee4e7bb8 725 led_ind3 = 0;
sayzyas 2:d0a5ee4e7bb8 726 }
sayzyas 2:d0a5ee4e7bb8 727 else if (( sw_validpart == 0 )&&( sw_ikmode == 1 )){
sayzyas 2:d0a5ee4e7bb8 728 // 0123456789012345
sayzyas 2:d0a5ee4e7bb8 729 // lcd_out2(0,0," [O][X] | JS K ",16);
sayzyas 2:d0a5ee4e7bb8 730 lcd_out2(0,0,"Crawler Para ",16);
sayzyas 2:d0a5ee4e7bb8 731 led_crawler_valid(counter);
sayzyas 2:d0a5ee4e7bb8 732 led_ind0 = 0;
sayzyas 2:d0a5ee4e7bb8 733 led_ind1 = 0;
sayzyas 2:d0a5ee4e7bb8 734 led_ind2 = 0;
sayzyas 2:d0a5ee4e7bb8 735 led_ind3 = 0;
sayzyas 2:d0a5ee4e7bb8 736 }
sayzyas 2:d0a5ee4e7bb8 737 else if (( sw_validpart == 1 )&&( sw_ikmode == 0 )){
sayzyas 2:d0a5ee4e7bb8 738 // 0123456789012345
sayzyas 2:d0a5ee4e7bb8 739 // lcd_out2(0,0," [X][O] | JS I ",16);
sayzyas 2:d0a5ee4e7bb8 740 lcd_out2(0,0,"Winch ",16);
sayzyas 2:d0a5ee4e7bb8 741 led_winch_valid(counter);
sayzyas 2:d0a5ee4e7bb8 742 led_ind4 = 0;
sayzyas 2:d0a5ee4e7bb8 743 led_ind5 = 0;
sayzyas 2:d0a5ee4e7bb8 744 led_ind6 = 0;
sayzyas 2:d0a5ee4e7bb8 745 led_ind7 = 0;
sayzyas 2:d0a5ee4e7bb8 746 }
sayzyas 2:d0a5ee4e7bb8 747 else if (( sw_validpart == 1 )&&( sw_ikmode == 1 )){
sayzyas 2:d0a5ee4e7bb8 748 // 0123456789012345
sayzyas 2:d0a5ee4e7bb8 749 // lcd_out2(0,0," [X][O] | JS K ",16);
sayzyas 2:d0a5ee4e7bb8 750 lcd_out2(0,0,"Winch ",16);
sayzyas 2:d0a5ee4e7bb8 751 led_winch_valid(counter);
sayzyas 2:d0a5ee4e7bb8 752 led_ind4 = 0;
sayzyas 2:d0a5ee4e7bb8 753 led_ind5 = 0;
sayzyas 2:d0a5ee4e7bb8 754 led_ind6 = 0;
sayzyas 2:d0a5ee4e7bb8 755 led_ind7 = 0;
sayzyas 2:d0a5ee4e7bb8 756 }
sayzyas 2:d0a5ee4e7bb8 757
sayzyas 2:d0a5ee4e7bb8 758 #ifdef __UNUSE__
sayzyas 2:d0a5ee4e7bb8 759 // ----------------------------------------------------------------
sayzyas 2:d0a5ee4e7bb8 760 // i2c slave read
sayzyas 2:d0a5ee4e7bb8 761 // ----------------------------------------------------------------
sayzyas 0:f4e2a62331ef 762 i = slave.receive();
sayzyas 2:d0a5ee4e7bb8 763 slave.read(buf, NumberOfI2CCommand ); /* Read size should be 10bit ?? */
sayzyas 0:f4e2a62331ef 764 switch (i) {
sayzyas 0:f4e2a62331ef 765 case I2CSlave::NoData:
sayzyas 0:f4e2a62331ef 766 // DEBUG_PRINT_L0("Handy> the slave has not been addressed\r\n");
sayzyas 2:d0a5ee4e7bb8 767 // lcd_out2(0,0,"System OK[-] ",16);
sayzyas 0:f4e2a62331ef 768 break;
sayzyas 0:f4e2a62331ef 769 case I2CSlave::ReadAddressed:
sayzyas 0:f4e2a62331ef 770 DEBUG_PRINT_L0("Handy> the master has requested a read from this slave\r\n");
sayzyas 0:f4e2a62331ef 771 slave.write(msg, strlen(msg) + 1); // Includes null char
sayzyas 0:f4e2a62331ef 772 break;
sayzyas 0:f4e2a62331ef 773 case I2CSlave::WriteGeneral:
sayzyas 0:f4e2a62331ef 774 DEBUG_PRINT_L0("Handy> the master is writing to all slave\r\n");
sayzyas 0:f4e2a62331ef 775 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:f4e2a62331ef 776 DEBUG_PRINT_L0("Handy> Read G: %s\r\n", buf);
sayzyas 0:f4e2a62331ef 777 break;
sayzyas 0:f4e2a62331ef 778 case I2CSlave::WriteAddressed:
sayzyas 0:f4e2a62331ef 779 DEBUG_PRINT_L0("Handy> the master is writing to this slave\r\n");
sayzyas 0:f4e2a62331ef 780 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:f4e2a62331ef 781 // DEBUG_PRINT_L0("Handy> Read A: %s\r\n", buf);
sayzyas 0:f4e2a62331ef 782 if( buf[5] == 0x01 ){
sayzyas 2:d0a5ee4e7bb8 783 // lcd_out2(0,0,"System OK[|] ",16); // command
sayzyas 0:f4e2a62331ef 784 }
sayzyas 0:f4e2a62331ef 785 else if( buf[5] == 0x04 ){
sayzyas 2:d0a5ee4e7bb8 786 // lcd_out2(0,0,"ERROR ",16);
sayzyas 0:f4e2a62331ef 787 }
sayzyas 0:f4e2a62331ef 788 else{
sayzyas 2:d0a5ee4e7bb8 789 // lcd_out2(0,0,"System OK[/] ",16); // unknown command
sayzyas 0:f4e2a62331ef 790 }
sayzyas 0:f4e2a62331ef 791 break;
sayzyas 0:f4e2a62331ef 792 }
sayzyas 0:f4e2a62331ef 793 for(int i = 0; i < 10; i++) buf[i] = 0; // Clear buffer
sayzyas 2:d0a5ee4e7bb8 794 #endif
sayzyas 0:f4e2a62331ef 795 counter++;
sayzyas 2:d0a5ee4e7bb8 796 if( counter >= 100 ){
sayzyas 2:d0a5ee4e7bb8 797 // led1 = !led1;
sayzyas 0:f4e2a62331ef 798 counter = 0;
sayzyas 0:f4e2a62331ef 799 }
sayzyas 0:f4e2a62331ef 800 Thread::wait(5);
sayzyas 0:f4e2a62331ef 801 }
sayzyas 0:f4e2a62331ef 802 }