firmware/src/main.cpp
//callback function when a control message is received
callback function when a control message is received
callback function when a control message is received
void rover_control(const geometry_msgs::Twist& ctrl_msg)
{ 
  double left_motor_pid = ctrl_msg.linear.x;
  double right_motor_pid = ctrl_msg.angular.z;
  int32_t left_motor_pwm = constrain(left_motor_pid, -255, 255);
  int32_t right_motor_pwm = constrain(right_motor_pid, -255, 255);
  if (left_motor_pwm < -1){
    left_motor.backward();
  }
  else if(left_motor_pwm>1){
    left_motor.forward();
  }
  else{
    left_motor.stop();
  }
  if (right_motor_pwm < -1){
    right_motor.backward();
  }
  else if(right_motor_pwm>1){
    right_motor.forward();
  }
  else{
    right_motor.stop();
  }
  left_motor.setSpeed(abs(left_motor_pwm));
  right_motor.setSpeed(abs(right_motor_pwm));
}
void rover_control ( const geometry_msgs :: Twist & ctrl_msg ) { double left_motor_pid = ctrl_msg . linear . x ; double right_motor_pid = ctrl_msg . angular . z ; int32_t left_motor_pwm = constrain ( left_motor_pid , -255 , 255 ) ; int32_t right_motor_pwm = constrain ( right_motor_pid , -255 , 255 ) ; if ( left_motor_pwm < -1 ) { left_motor . backward ( ) ; } else if ( left_motor_pwm > 1 ) { left_motor . forward ( ) ; } else { left_motor . stop ( ) ; } if ( right_motor_pwm < -1 ) { right_motor . backward ( ) ; } else if ( right_motor_pwm > 1 ) { right_motor . forward ( ) ; } else { right_motor . stop ( ) ; } left_motor . setSpeed ( abs ( left_motor_pwm ) ) ; right_motor . setSpeed ( abs ( right_motor_pwm ) ) ; }
callback function when a control message is received
callback function when a control message is received
// Serial1.println(left_motor_pwm); // Serial1.println(right_motor_pwm);