void stop(){
SpeedValue_left = 120;
SpeedValue_right = 120;
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");
}
void right(){
SpeedValue_left = 140;
SpeedValue_right = 105;
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");
}
void backward(){
SpeedValue_left = 105;
SpeedValue_right = 105;
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");
}
void forward(){
SpeedValue_left = 140;
SpeedValue_right = 140;
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");
}
/* test use for printing the status and counter value through usart */
void getEncoder(void){
detachEXTI(EXTI_Line0 | EXTI_Line1 | EXTI_Line2 | EXTI_Line3);
USART_puts(USART3, "L_state:");
USART_putd(USART3, ENCODER_L.rotate);
USART_puts(USART3, " R_state:");
USART_putd(USART3, ENCODER_R.rotate);
getEncoderState(&ENCODER_L);
getEncoderState(&ENCODER_R);
USART_puts(USART3, " L_state2:");
USART_putd(USART3, ENCODER_L.rotate);
USART_puts(USART3, " R_state2:");
USART_putd(USART3, ENCODER_R.rotate);
USART_puts(USART3, " le_en:");
USART_putd(USART3, ENCODER_L.count);
USART_puts(USART3, " ri_en:");
USART_putd(USART3, ENCODER_R.count);
USART_puts(USART3, "\r\n");
/* clear encoder counter*/
ENCODER_L.count = 0;
ENCODER_R.count = 0;
attachEXTI(EXTI_Line0 | EXTI_Line1 | EXTI_Line2 | EXTI_Line3);
}
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();
}
}
}
void receive_pi_command(){
for(i = 0; i < 3; i++){
pos_received_x[i] = pi_received_string[i];
pos_received_y[i] = pi_received_string[i+3];
}
if(pos_received_x[0]=='0'){
pos_received_x1[0] = pos_received_x[1];
pos_received_x1[1] = pos_received_x[2];
pts_x = atoi(pos_received_x1);
motorStop();
}
else
pts_x = atoi(pos_received_x);
if(pos_received_y[0]=='0'){
pos_received_y1[0] = pos_received_y[1];
pos_received_y1[1] = pos_received_y[2];
pts_y = atoi(pos_received_y1);
motorStop();
}
else
pts_y = atoi(pos_received_y);
//USART_puts(USART6, "\r\nPi receive test\n\r");
// pts_x is the position on image plane
// actual position of object
// pts_x = XXX * pts_x
// degree of lidar data
lidar_Pos = pts_x*9/32;
//motorForward();
object_distance = Lidar_distance[lidar_Pos];
#if 0
USART_puts(USART6, "\r\nX:");
USART_putd(USART6, pts_x);
USART_puts(USART6, " Y:");
USART_putd(USART6, pts_y);
USART_puts(USART6, "\r\nlidar_Pos:");
USART_putd(USART6, lidar_Pos);
USART_puts(USART6, "\r\nobject_distance:");
USART_putd(USART6, object_distance);
#endif
#if 1
// Motor moving
if(pts_x < 320){
// motorLeft(lValue, rValue)
Left();
//vTaskDelay(300);
}
else
{
Right();
//vTaskDelay(300);
}
//motorStop();
//vTaskDelay(10);
#endif
for(i = 0; i < 3; i++){
pos_received_x[i] = 0;
pos_received_y[i] = 0;
}
pts_x = 0;
pts_y = 0;
}