// Sets the color of the RGB LEB
void RGB::setColor(int red, int green, int blue) {
int r = capValue(red);
if (r != _red) {
_red = r;
analogWrite(_redPin, _red);
}
int g = capValue(green);
if (g != _green) {
_green = g;
analogWrite(_greenPin, _green);
}
int b = capValue(blue);
if (b != _blue) {
_blue = b;
analogWrite(_bluePin, _blue);
}
}
geometry_msgs::Twist
GpsMover::createTwistMessage(geometry_msgs::Point current_location,
double current_heading,
geometry_msgs::Point waypoint) {
// The command to return
geometry_msgs::Twist command;
// Set components we don't care about to 0
command.linear.y = 0;
command.linear.z = 0;
command.angular.x = 0;
command.angular.y = 0;
// Figure out the minimum we have to turn to point directly at the waypoint
double angle_to_waypoint = angleBetweenPoints(current_location, waypoint);
double min_turning_angle =
minAngularChange(current_heading, angle_to_waypoint);
// Figure out how far we are from the waypoint
double dx = waypoint.x - current_location.x;
double dy = waypoint.y - current_location.y;
double distance = sqrt(pow(dx, 2) + pow(dy, 2));
// Figure out how fast we should turn
command.angular.z = magicFunction(
distance, min_turning_angle, linear_distance_factor, linear_heading_factor);
// Figure out if we should be turning left or right
command.angular.z *= (min_turning_angle > 0) ? 1 : -1;
// Figure out how fast we should move forward
command.linear.x = magicFunction(
min_turning_angle, distance, linear_heading_factor, linear_distance_factor);
// Cap our angular and linear speeds
capValue(command.linear.x, max_linear_speed);
capValue(command.angular.z, max_angular_speed);
return command;
}
void output()
{
mtrTarget[D_LEFT] = outDrvL;
mtrTarget[D_RIGHT] = outDrvR;
for (int j=0; j<3; j++)
{
#ifdef SLEW
mtrSlewed[j] += slew(mtrTarget[j], mtrSlewed[j], slewConstants[j]); //SLEW CONTROLLERS
#endif
capValue(-100, mtrSlewed[j], 100); //CAP ALL MOTORS
motor[j] = mtrSlewed[j]; //ASSIGN MOTORS
}
string temp1,temp2;
StringFormat(temp1,"Time:%.1f",((float)autoTimer/1000));
StringFormat(temp2, "Step: %d", autoStep); //Show step
nxtDisplayCenteredTextLine(0,temp1);
nxtDisplayCenteredTextLine(1,temp2);
}
