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