static void detect_LS_Polling(){
/*detect actuator A LS*/
switch(actuator_state_A){
case ACTUATOR_STATE_IDLE:
break;//wait the next state into.
case ACTUATOR_STATE_MOVE_CW:
if(get_Linear_Actuator_A_LS_State()==0x01 || get_Linear_Actuator_A_LS_State()==0x03){
set_linearActuator_A_cmd(LINEAR_ACTU_STOP,0);
actuator_state_B=ACTUATOR_STATE_IDLE;
}
break;
case ACTUATOR_STATE_MOVE_CCW:
if(get_Linear_Actuator_A_LS_State()==0x02 || get_Linear_Actuator_A_LS_State()==0x03){
set_linearActuator_A_cmd(LINEAR_ACTU_STOP,0);
actuator_state_A=ACTUATOR_STATE_IDLE;
}
default:
break;
}
/*detect actuator B LS*/
switch(actuator_state_B){
case ACTUATOR_STATE_IDLE:
break;//wait the next command into.
case ACTUATOR_STATE_MOVE_CW:
if(get_Linear_Actuator_B_LS_State()==0x01 || get_Linear_Actuator_B_LS_State()==0x03){
set_linearActuator_B_cmd(LINEAR_ACTU_STOP,0);
actuator_state_B=ACTUATOR_STATE_IDLE;
}
break;
case ACTUATOR_STATE_MOVE_CCW:
if(get_Linear_Actuator_B_LS_State()==0x02 || get_Linear_Actuator_B_LS_State()==0x03){
set_linearActuator_B_cmd(LINEAR_ACTU_STOP,0);
actuator_state_B=ACTUATOR_STATE_IDLE;
}
break;
default:
break;
}
}
void PerformCommand(unsigned char group,unsigned char control_id, unsigned char value) //bt_forward:turnOn=0;moveWheelchair='d';moveForward='f'
{
static int actuator_pwm_value = 0;
if(group == OUT_EPW_CMD){ /*0*/
switch ( control_id )
{
case EPW_MOTOR_DIR: //direction //moveWheelchair //EPW_MOTOR_DIR=100,equal to char 'd'
parse_EPW_motor_dir(value);
//parse_Joystick_dir();
break;
case EPW_MOTOR_PWM: //speed //EPW_MOTOR_PWM=101,equal to char 'e' //sb_Speed:turnOn, setSpeed='e', speedValue
motor_speed_value = value; /*0~10 scale*/ //can't find motor_speed_value
break;
case EPW_ACTUATOR_A : //ACTUATOR_A
set_linearActuator_A_cmd(value , actuator_pwm_value); /*the actuator of pwm_value is fixed, value is dir flag.*/
break;
case EPW_ACTUATOR_B : //ACTUATOR_B
set_linearActuator_B_cmd(value , actuator_pwm_value); /*the actuator of pwm_value is fixed. value is dir flag.*/
break;
default:
break;
}
}
}
void receive_task(){
if(Receive_String_Ready){
//forward step by step
if(received_string[0] == '+'){
SpeedValue_left += inc;
SpeedValue_right += inc;
mMove(SpeedValue_left,SpeedValue_right);
USART_puts(USART3, "left:");
USART_putd(USART3, SpeedValue_left);
USART_puts(USART3, " right:");
USART_putd(USART3, SpeedValue_right);
USART_puts(USART3, "\r\n");
}
//backward step by step
else if(received_string[0] == '-'){
SpeedValue_left -= inc;
SpeedValue_right -= inc;
mMove(SpeedValue_left, SpeedValue_right);
USART_puts(USART3, "left:");
USART_putd(USART3, SpeedValue_left);
USART_puts(USART3, " right:");
USART_putd(USART3, SpeedValue_right);
USART_puts(USART3, "\r\n");
}
//forward
else if(received_string[0] == 'p'){
forward();
}
//PID
else if(received_string[0] == 'f'){
test_PID_forward();
}
//backward
else if(received_string[0] == 'b'){
test_PID_backward();
}
//left
else if(received_string[0] == 'l'){
test_PID_left();
}
//right
else if(received_string[0] == 'r'){
test_PID_right();
}
//stop
else if(received_string[0] == 's'){
stop();
}
//Linear Acturator
else if(received_string[0] == 'a'){
USART_puts(USART3, "Actu_A_up");
set_linearActuator_A_cmd(LINEAR_ACTU_CW);
USART_puts(USART3, "\r\n");
}
else if(received_string[0] == 'n'){
USART_puts(USART3, "Actu_A_down");
//set_linearActuator_A_cmd(LINEAR_ACTU_CCW);
USART_puts(USART3, "\r\n");
set_linearActuator_A_cmd(LINEAR_ACTU_CCW);
}
else if(received_string[0] == 'd'){
USART_puts(USART3, "Actu_A_stop");
//set_linearActuator_A_cmd(LINEAR_ACTU_CCW);
USART_puts(USART3, "\r\n");
set_linearActuator_A_cmd(LINEAR_ACTU_STOP);
}
else if(received_string[0] == 'u'){
USART_puts(USART3, "Actu_B_up");
set_linearActuator_B_cmd(LINEAR_ACTU_CW);
USART_puts(USART3, "\r\n");
}
else if(received_string[0] == 'k'){
USART_puts(USART3, "Actu_B_down");
//set_linearActuator_B_cmd(LINEAR_ACTU_CCW);
USART_puts(USART3, "\r\n");
set_linearActuator_B_cmd(LINEAR_ACTU_CCW);
}
else if(received_string[0] == 'w'){
USART_puts(USART3, "Actu_B_stop");
//set_linearActuator_B_cmd(LINEAR_ACTU_CCW);
USART_puts(USART3, "\r\n");
set_linearActuator_B_cmd(LINEAR_ACTU_STOP);
}
//get encoder
else if(received_string[0] == 'e'){
getEncoder();
}
//test
else if(received_string[0] == 't'){
test_forward();
}
}
}