void motor_c_dir(MotorDirection dir) { dm_c.dir = dir << MOTOR_C_SHIFT; dlcd_hide(LCD_C_LEFT); dlcd_hide(LCD_C_RIGHT); if (dir == fwd || dir == brake) dlcd_show(LCD_C_RIGHT); if (dir == rev || dir == brake) dlcd_show(LCD_C_LEFT); }
//! receive a byte, decoding LNP packets with a state machine. void lnp_integrity_byte(unsigned char b) { static unsigned char buffer[256+3]; static int bytesRead,endOfData; static unsigned char chk; if(lnp_integrity_state==LNPwaitHeader) bytesRead=0; buffer[bytesRead++]=b; switch(lnp_integrity_state) { case LNPwaitHeader: // valid headers are 0xf0 .. 0xf7 // if(((b & 0xf8) == 0xf0) || (b == 0x55)) { #ifdef CONF_VIS if (lnp_logical_range_is_far()) { dlcd_show(LCD_IR_UPPER); dlcd_show(LCD_IR_LOWER); } else { dlcd_hide(LCD_IR_UPPER); dlcd_show(LCD_IR_LOWER); } #ifndef CONF_LCD_REFRESH lcd_refresh(); #endif #endif // Init checksum lnp_checksum_init( chk ); // switch on protocol header if (b == 0x55) { #if defined(CONF_RCX_PROTOCOL) || defined(CONF_RCX_MESSAGE) // 0x55 is header for standard firmware message lnp_integrity_state = LNPwaitRMH1; #else lnp_integrity_reset(); #endif } else { lnp_integrity_state++; } } break; case LNPwaitLength: endOfData=b+2; lnp_integrity_state++; break; case LNPwaitData: if(bytesRead==endOfData) lnp_integrity_state++; break; case LNPwaitCRC: if(b==chk) lnp_receive_packet(buffer); lnp_integrity_reset(); break; #if defined(CONF_RCX_PROTOCOL) || defined (CONF_RCX_MESSAGE) // state machine to handle remote case LNPwaitRMH1: case LNPwaitRMH2: // waiting for header bytes if ( b == lnp_rcx_header[ lnp_integrity_state-LNPwaitRMH1 ] ) lnp_integrity_state++; else lnp_integrity_reset(); break; case LNPwaitRMH3: case LNPwaitRMH4: if ( b == lnp_rcx_remote_op[ lnp_integrity_state-LNPwaitRMH3 ] ) lnp_integrity_state++; #if defined(CONF_RCX_MESSAGE) else if ( b == lnp_rcx_msg_op[ lnp_integrity_state-LNPwaitRMH3 ] ) lnp_integrity_state = LNPwaitMH4; #endif else lnp_integrity_reset(); break; case LNPwaitRB0: lnp_rcx_temp0 = b; lnp_integrity_state++; break; case LNPwaitRB0I: if ( (unsigned char)~b == lnp_rcx_temp0 ) lnp_integrity_state++; else lnp_integrity_reset(); break; case LNPwaitRB1: lnp_rcx_temp1 = b; lnp_integrity_state++; break; case LNPwaitRB1I: if ( (unsigned char)~b == lnp_rcx_temp1 ) lnp_integrity_state++; else lnp_integrity_reset(); break; case LNPwaitRC: lnp_rcx_checksum = 0xd2 + lnp_rcx_temp0 + lnp_rcx_temp1; if ( b == lnp_rcx_checksum ) lnp_integrity_state++; else lnp_integrity_reset(); break; case LNPwaitRCI: // if checksum valid and remote handler has been installed, call remote handler if ( b == (unsigned char)~lnp_rcx_checksum) { #if defined(CONF_RCX_MESSAGE) // if a message, set message number and exit if (lnp_rcx_temp1 & 0x07) { lnp_rcx_message = (lnp_rcx_temp1 > 2) ? 3 : lnp_rcx_temp1; } else #endif { // Invoke remote handler if any lnp_remote_handler_t rmth = lnp_remote_handler; if (rmth) rmth( (lnp_rcx_temp0<<8)+lnp_rcx_temp1 ); } } // reset state machine when done lnp_integrity_reset(); break; #endif #if defined(CONF_RCX_MESSAGE) // state machine to handle RCX protocol messages case LNPwaitMH3: case LNPwaitMH4: if ( b == lnp_rcx_msg_op[ lnp_integrity_state-LNPwaitMH3 ] ) lnp_integrity_state++; else lnp_integrity_reset(); break; case LNPwaitMN: lnp_rcx_temp0 = b; lnp_integrity_state++; break; case LNPwaitMNC: if ( (unsigned char)~b == lnp_rcx_temp0 ) lnp_integrity_state++; else lnp_integrity_reset(); break; case LNPwaitMC: lnp_rcx_temp1 = 0xf7 + lnp_rcx_temp0; if (b == lnp_rcx_temp1) lnp_integrity_state++; else lnp_integrity_reset(); break; case LNPwaitMCC: // set message variable if it is valid message if ( (unsigned char)~b == lnp_rcx_temp1 ) lnp_rcx_message = lnp_rcx_temp0; // reset state machine lnp_integrity_reset(); break; #endif } // Accumulate checksum lnp_checksum_step( chk, b ); }