// 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); }