void processMotors(struct OUTPUT_STRUCT output){ #if defined(SPYDER_EN) int op1 = output.throttle + output.roll - output.pitch + output.yaw; int op2 = output.throttle - output.roll - output.pitch - output.yaw; int op3 = output.throttle - output.roll + output.pitch + output.yaw; int op4 = output.throttle + output.roll + output.pitch - output.yaw; withinBounds(op1, THROTTLE_MAXIMUM, THROTTLE_MINIMUM); withinBounds(op2, THROTTLE_MAXIMUM, THROTTLE_MINIMUM); withinBounds(op3, THROTTLE_MAXIMUM, THROTTLE_MINIMUM); withinBounds(op4, THROTTLE_MAXIMUM, THROTTLE_MINIMUM); if (output.throttle > THROTTLE_CUTOFF){ output1.writeMicroseconds(op1); output2.writeMicroseconds(op2); output3.writeMicroseconds(op3); output4.writeMicroseconds(op4); } else { output1.writeMicroseconds(THROTTLE_MINIMUM); output2.writeMicroseconds(THROTTLE_MINIMUM); output3.writeMicroseconds(THROTTLE_MINIMUM); output4.writeMicroseconds(THROTTLE_MINIMUM); } #elif defined(TRI_EN) int op1 = output.throttle + output.roll - 0.8 * output.pitch; int op2 = output.throttle - output.roll - 0.8 * output.pitch; int op3 = output.throttle + output.pitch; int op4 = SERVO_MIDPOINT + output.yaw + 30; double tailConv = intMap(op4, TAIL_SERVO_MIN, TAIL_SERVO_MAX, TAIL_SERVO_MIN_DEGREE, TAIL_SERVO_MAX_DEGREE) - TAIL_SERVO_OFFSET; op3 = ((op3 - SERVO_MINIMUM) / sin(radians(tailConv))) + SERVO_MINIMUM; withinBounds(op1, THROTTLE_MAXIMUM, THROTTLE_MINIMUM); withinBounds(op2, THROTTLE_MAXIMUM, THROTTLE_MINIMUM); withinBounds(op3, THROTTLE_MAXIMUM, THROTTLE_MINIMUM); withinBounds(op4, THROTTLE_MAXIMUM, THROTTLE_MINIMUM); if (output.throttle > THROTTLE_CUTOFF){ output1.writeMicroseconds(op1); output2.writeMicroseconds(op2); output3.writeMicroseconds(op3); output4.writeMicroseconds(op4); } else { output1.writeMicroseconds(THROTTLE_MINIMUM); output2.writeMicroseconds(THROTTLE_MINIMUM); output3.writeMicroseconds(THROTTLE_MINIMUM); if (TRI_EN){ output4.writeMicroseconds(SERVO_MIDPOINT); } else { output4.writeMicroseconds(THROTTLE_MINIMUM); } } #elif defined(ROVER_EN) int op2 = output.throttle; int op4 = output.yaw; withinBounds(op2, THROTTLE_MAXIMUM, THROTTLE_MINIMUM); withinBounds(op4, THROTTLE_MAXIMUM, THROTTLE_MINIMUM); output2.writeMicroseconds(op2); output4.writeMicroseconds(op4); #endif }
/*! * \brief Redraws the visualizer screen */ void DishVisualizer::redraw() { while (isInit) { for (uint showChan = 0; showChan < data.size(); showChan++) { uint16_t red = 0; uint16_t green = 0; uint16_t blue = 0; switch (color_mode) { // Red/blue separated: // Channels at/under threshold are a red gradient // Channels above threshold are solid blue case RED_BLUE_SEPARATED: if (data[showChan] <= thresholds[showChan]) red = intMap(data[showChan], min_volts[showChan], thresholds[showChan], MAX_COLOR / 2, 0); else blue = MAX_COLOR; break; // Red/blue mix: // Channels have a mix of red and blue. Mostly red // indicates a low value and mostly blue indicates a // high value. case RED_BLUE_MIX: red = intMap(data[showChan], min_volts[showChan], max_volts[showChan], MAX_COLOR, 0); blue = intMap(data[showChan], min_volts[showChan], max_volts[showChan], 0, MAX_COLOR); break; // Red/green/blue: // Channels at/under baseline are a red gradient // Channels above baseline and at/under threshold are // a green gradient // Channels above threshold are solid blue case RED_GREEN_BLUE: if (data[showChan] <= baselines[showChan]) red = intMap(data[showChan], min_volts[showChan], thresholds[showChan], MAX_COLOR, 0); else if (data[showChan] <= thresholds[showChan]) green = intMap(data[showChan], min_volts[showChan], thresholds[showChan], MAX_COLOR, 0); else blue = MAX_COLOR; break; // Blue only: // Channels at/under threshold are black // Channels above threshold are solid blue case BLUE_ONLY: if (data[showChan] > thresholds[showChan]) blue = MAX_COLOR; break; default: ROS_ERROR("Color mode (%d) is out of range [0...3]", color_mode); color_mode = RED_BLUE_SEPARATED; break; } plotter->color(red, green, blue); //Get the center coordinates and draw the circle int xPos = centers[showChan][0]; int yPos = centers[showChan][1]; //ROS_INFO("%d = %f: %d, %d, %d at (%d, %d)", showChan, data[showChan], red, green, blue, xPos, yPos); plotter->circle(xPos, yPos, RADIUS); } // Plot CA if it has been updated if (plot_ca) { int x_coord = static_cast<int>((ca.x - 4.5) * X_STEP) + P_WIDTH / 2; int y_coord = P_HEIGHT / 2 - static_cast<int>((ca.y - 4.5) * Y_STEP); plotter->colorname("orange"); plotter->circle(x_coord, y_coord, RADIUS); plot_ca = false; } // Draw lines to separate grid into quadrants plotter->pencolorname("white"); plotter->line(P_WIDTH / 2, 0, P_WIDTH / 2, P_HEIGHT); plotter->line(0, P_HEIGHT / 2, P_WIDTH, P_HEIGHT / 2); plotter->endpath(); plotter->erase(); //sleep for a 60th of a second boost::this_thread::sleep(boost::posix_time::millisec(16)); } }