void SetMotorSpeedRight(float speed) {
char myString[64];
if (pidDrive) {
if (fabs(speed) < 0.5) {
speed = 0;
}
sprintf(myString, "pid SP R: %5.2f\n", speed);
SmartDashboard::PutString("DB/String 9", myString);
controlRight->SetSetpoint(speed);
} else {
sprintf(myString, "setpoint R: %5.2f\n", speed);
SmartDashboard::PutString("DB/String 9", myString);
m_pscMotorRight->Set(speed);
}
}
/**
* This function gets called when the robot wants to change the
* target speed or direction.
*/
void onVelocityUpdate(const geometry_msgs::TwistConstPtr& new_msg)
{
/* Set the target speed for the PID controller. */
ROS_INFO("New target speed is %.2f", new_msg->linear.x);
controller.setTarget(new_msg->linear.x);
/* Update the steering value, if it has changed. Remember that, as
* described above, this node intentionally "misinterprets" the
* angular.z value as a servo position for simplicity. */
int new_steer_value = steering_ctl.getRawValue(new_msg->angular.z);
if ( steering_cmd.value != new_steer_value ) {
steering_cmd.value = new_steer_value;
cmd_pub.publish(steering_cmd);
ROS_INFO("New steering value is %.2f", new_msg->angular.z);
}
}
/**
* This function gets called when the microcontroller publishes new
* state information.
*/
void onStateUpdate(const sb_msgs::RobotStateConstPtr& new_state)
{
odometer.onNewReading(new_state->odometer);
double time = ros::Time::now().toSec();///replace with t.to_nsec for nono seconds
double distance = odometer.getDistance();
double throttle = controller.onNewReading(distance, time);
if (throttle < 0.0) {
throttle = sb_util::clamp(throttle, -MAX_THROTTLE, -MIN_THROTTLE);
}
else {
throttle = sb_util::clamp(throttle, MIN_THROTTLE, MAX_THROTTLE);
}
throttle_cmd.value = throttle_ctl.getRawValue(throttle);
cmd_pub.publish(throttle_cmd);
ROS_INFO("%5.2f, %.2f, %.2f, %i",
time, distance, throttle, throttle_cmd.value);
}
void SetMotorSpeedLeft(float speed) {
char myString[64];
if (pidDrive) {
if (fabs(speed) < 0.5) {
speed = 0;
}
sprintf(myString, "pid SP L: %5.2f\n", -speed);
SmartDashboard::PutString("DB/String 8", myString);
controlLeft->SetSetpoint(-speed);
} else {
sprintf(myString, "setpoint L: %5.2f\n", -speed);
SmartDashboard::PutString("DB/String 8", myString);
m_pscMotorLeft->Set(-speed);
}
}
int main(int argc, char** argv)
{
/* Initialize node */
ROS_INFO("Starting %s", NODE_NAME.c_str());
ros::init(argc, argv, NODE_NAME);
ros::NodeHandle node;
/* Load configuration file */
std::string cfgfile;
if (! sb_config::findConfigFile(argc, argv, cfgfile) ) {
ROS_FATAL("Can't find configuration file.");
ros::shutdown();
}
ROS_INFO("Loading configuration in %s", cfgfile.c_str());
throttle_ctl.loadConfig(cfgfile);
steering_ctl.loadConfig(cfgfile);
odometer.loadConfig(cfgfile);
controller.loadConfig(cfgfile);
/* Initialize the ServoCommand messages. */
throttle_cmd.id = throttle_ctl.getId();
throttle_cmd.value = 0;
steering_cmd.id = steering_ctl.getId();
steering_cmd.value = 0;
/* Create publisher and subscribers */
cmd_pub = node.advertise<sb_msgs::ServoCommand>(
CMD_TOPIC, MSG_QUEUE_SIZE
);
ros::Subscriber spd_sub = node.subscribe(
SPEED_TOPIC, MSG_QUEUE_SIZE, onVelocityUpdate
);
ros::Subscriber state_sub = node.subscribe(
STATE_TOPIC, MSG_QUEUE_SIZE, onStateUpdate
);
/* Run the main loop */
ROS_INFO("time, distance, throttle, servo");
while (ros::ok()) {
ros::spin();
}
ROS_INFO("Shutting down %s", NODE_NAME.c_str());
}
void PIDWrite(float output) {
motor->Set(motor->Get() + mult * output);
//printf("pid drive enabled");
}
void Set(float _speed, UINT8 syncGroup=0) {
speed = _speed;
if (this->SendablePIDController::IsEnabled()) SendablePIDController::SetSetpoint(speed * scale);
//else motor->Set(speed);
motor->Set(speed);
}
void Reset() {
m_speedController->Set(0.0f);
m_encoder->Reset();
}
void LiftPIDOutput::PIDWrite(float output)
{
m_liftMotor1->Set(output + LIFTNEUTRALPOWER);
m_liftMotor2->Set(output + LIFTNEUTRALPOWER);
}
void GripPIDOutput::PIDWrite(float output)
{
m_motor1->Set(output);
//m_motor2->Set(-output);
}
