void motor_control(){
float x_value,y_value;
float x1,x2,y1,y2,motor_left,motor_right;
float left_pwm,right_pwm;
int dont_move = 0;
//References from transmitter
/*XbeeTxBuffer[9] = (map(analogRead(joy_2_x),0,1023,100,0))/4; //Byte 2
XbeeTxBuffer[10] = (map(analogRead(joy_2_y),0,1023,100,0))/4; //Byte 3
XbeeTxBuffer[11] = (map(analogRead(joy_1_x),0,1023,100,0))/4; //Byte 4
XbeeTxBuffer[12] = (map(analogRead(joy_1_y),0,1023,100,0))/4; //Byte 5*/
if(good_comm == 1){
x_value = RxBuffer[9];
y_value = RxBuffer[10];
//left_pwm = ((y_value - 128) * 2) + ((x_value - 128) * 2);
//right_pwm = ((y_value - 128) * 2) + ((128 - x_value) * 2);
if(x_value >= 124){
if(x_value <= 132){
if(y_value >= 124){
if(y_value <= 132){
digitalWrite(pwm1, LOW);
digitalWrite(pwm2, LOW);
digitalWrite(pwm3, LOW);
digitalWrite(pwm4, LOW);
digitalWrite(dir1, LOW);
digitalWrite(dir2, LOW);
digitalWrite(dir3, LOW);
digitalWrite(dir4, LOW);
dont_move = 1;
}
}
}
}
if(dont_move == 0){
left_pwm = ((y_value - 128) * 2) + ((128 - x_value) * 2);
right_pwm = ((y_value - 128) * 2) + ((x_value - 128) * 2);
//FRONT
if(left_pwm >= 0){
if(left_pwm >= 255){
left_pwm = 255;
}
digitalWrite(dir1, LOW);
digitalWrite(dir3, LOW);
analogWrite(pwm1, left_pwm);
analogWrite(pwm3, left_pwm);
}
//FRONT
if(right_pwm >= 0){
if(right_pwm >= 255){
right_pwm = 255;
}
digitalWrite(dir2, LOW);
digitalWrite(dir4, LOW);
analogWrite(pwm2, right_pwm);
analogWrite(pwm4, right_pwm);
}
//REVERSE
if(left_pwm < 0){
if(left_pwm <= -255){
left_pwm = -255;
}
left_pwm = -left_pwm;
digitalWrite(pwm1, LOW);
digitalWrite(pwm3, LOW);
analogWrite(dir1, left_pwm);
analogWrite(dir3, left_pwm);
}
//REVERSE
if(right_pwm < 0){
if(right_pwm <= -255){
right_pwm = -255;
}
right_pwm = -right_pwm;
digitalWrite(pwm2, LOW);
digitalWrite(pwm4, LOW);
analogWrite(dir2, right_pwm);
analogWrite(dir4, right_pwm);
}
}
}else{
digitalWrite(pwm1, LOW);
digitalWrite(pwm2, LOW);
digitalWrite(pwm3, LOW);
digitalWrite(pwm4, LOW);
digitalWrite(dir1, LOW);
digitalWrite(dir2, LOW);
digitalWrite(dir3, LOW);
digitalWrite(dir4, LOW);
}//END ELSE
dont_move = 0;
}//END FUNCTION