void setup_score_8_plus_8_poms_in_upper_storage() {
initialize_camera();
initialize_servos(); // FIXME: Make this take an argument to be robot-neutral.
// FIXME: Must ask for these at run-time.
_OUR_COLOR = PINK;
_THEIR_COLOR = GREEN;
set_mc_big_amount(10, 20);
set_mc_small_amount(2, 3);
set_mc_slow_speed(50, 80);
set_mc_very_slow_speed(30, 50);
turn_mc_off();
}
int main(int argc, char **argv) {
// Initialize ROS elements
ros::init(argc, argv, "manual_keyboard_control");
ros::NodeHandle n;
ros::Rate loop_rate(20);
// Publishers for sending commands to motor controller
arm_cmd_manual = new ros::Publisher();
steer_cmd_manual = new ros::Publisher();
drive_cmd_manual = new ros::Publisher();
mast_cmd_manual = new ros::Publisher();
*arm_cmd_manual = n.advertise<std_msgs::Int16MultiArray>("arm_cmd_manual", 1000);
*steer_cmd_manual = n.advertise<std_msgs::Int16MultiArray>("steer_cmd_manual", 1000);
*drive_cmd_manual = n.advertise<std_msgs::Int16MultiArray>("drive_cmd_manual", 1000);
*mast_cmd_manual = n.advertise<std_msgs::Int16>("mast_cmd_manual", 1000);
// Keyboard subscribers
ros::Subscriber keydown = n.subscribe("keyboard/keydown", 10, keyDown);
ros::Subscriber keyup = n.subscribe("keyboard/keyup", 10, keyUp);
// Other Initialization code
initialize_servos();
initialize_key_states();
std::cout << "STARTED PWM PUBLISHER!!!" << std::endl;
while (ros::ok())
{
// Arm home (p)
if (keys[keyboard::Key::KEY_p]) {
int target_angle = 0;
int angle_delta = 0;
// For each arm servo
for (int i = 0; i < 6; i++) {
// If the shoulder is extended and the wrist is not brought in
// then bring in the wrist (to reduce strain on shoulder servo)
// Which means if the current servo is not the wrist, skip it
// unless the wrist is retracted!
if (arm_servo[ARM_SHOULDER] > 55 && arm_servo[ARM_WRIST] != -40 && i != ARM_WRIST) {
continue;
}
if (i == ARM_BASE && arm_servo[ARM_BASE] != 0 && arm_servo[ARM_SHOULDER] == 30) {
// If current servo is the base, and the base is not at 0 degrees,
// and the should is currently at 30 degrees (to avoid hitting things)
// then set the base target angle to 0 degrees
target_angle = 0;
} else if (i == ARM_SHOULDER && arm_servo[ARM_BASE] != 0) {
// If the current servo is the shoulder, and the base is not 0 degrees,
// set the shoulder target angle to 30 degrees.
target_angle = 30;
} else if (i == ARM_ELBOW && (arm_servo[ARM_BASE] != 0 || arm_servo[ARM_SHOULDER] > 20)) {
// If the current servo is the elbow, and either the base is not at 0 or the
// shoulder is bigger than 20 degrees, set the target angle for the elbow
// to whatever the shoulder is plus 30 degrees (to avoid collisions)
target_angle = arm_servo[ARM_SHOULDER] + 30;
} else if (i == ARM_WRIST && arm_servo[ARM_SHOULDER] > 50) {
// If the current servo is the wrist, and the shoulder is at more than
// 50 degrees, set the wrist angle to -40
target_angle = 0;
} else if (i == ARM_GRIPPER_ROTATE) {
target_angle = 0;
} else if (i == ARM_GRIPPER_CLAW) {
continue;
} else {
// Otherwise set the target angle to "home" (ie 0 degrees)
target_angle = 0;
}
// Save change in angle
if (arm_servo[i] < target_angle) {
angle_delta = 1;
} else if (arm_servo[i] > target_angle) {
angle_delta = -1;
} else {
angle_delta = 0;
}
// Double speed base, wrist gripper rotate or gripper claw
if (i == ARM_BASE || i == ARM_WRIST || i == ARM_GRIPPER_ROTATE || i == ARM_GRIPPER_CLAW) {
if (arm_servo[i] - 1 != 0 && arm_servo[i] + 1 != 0) {
angle_delta *= 2;
}
}
// Publish change in angle
arm_servo[i] += angle_delta;
}
arm_update_needed = true;
}
// Arm base (m and n)
if (keys[keyboard::Key::KEY_n]) {
arm_servo[ARM_BASE] += 1;
arm_update_needed = true;
} else if (keys[keyboard::Key::KEY_m]) {
arm_servo[ARM_BASE] -= 1;
arm_update_needed = true;
}
// Arm shoulder (j and u)
if (keys[keyboard::Key::KEY_j]) {
arm_servo[ARM_SHOULDER] += 1;
arm_update_needed = true;
} else if (keys[keyboard::Key::KEY_u]) {
arm_servo[ARM_SHOULDER] -= 1;
arm_update_needed = true;
}
// Arm elbow (i and k)
if (keys[keyboard::Key::KEY_i]) {
arm_servo[ARM_ELBOW] += 1;
arm_update_needed = true;
} else if (keys[keyboard::Key::KEY_k]) {
arm_servo[ARM_ELBOW] -= 1;
arm_update_needed = true;
}
// Arm wrist (o and l)
if (keys[keyboard::Key::KEY_o]) {
arm_servo[ARM_WRIST] += 1;
arm_update_needed = true;
} else if (keys[keyboard::Key::KEY_l]) {
arm_servo[ARM_WRIST] -= 1;
arm_update_needed = true;
}
// Arm gripper rotate (left arrow and right arrow)
if (keys[keyboard::Key::KEY_LEFT]) {
if (arm_servo[ARM_GRIPPER_ROTATE] + GRIPPER_ROTATE_INCREMENT >= GRIPPER_ROTATE_MAX) {
arm_servo[ARM_GRIPPER_ROTATE] = GRIPPER_ROTATE_MAX;
} else {
arm_servo[ARM_GRIPPER_ROTATE] += GRIPPER_ROTATE_INCREMENT;
}
arm_update_needed = true;
} else if (keys[keyboard::Key::KEY_RIGHT]) {
if (arm_servo[ARM_GRIPPER_ROTATE] - GRIPPER_ROTATE_INCREMENT <= GRIPPER_ROTATE_MIN) {
arm_servo[ARM_GRIPPER_ROTATE] = GRIPPER_ROTATE_MIN;
} else {
arm_servo[ARM_GRIPPER_ROTATE] -= GRIPPER_ROTATE_INCREMENT;
}
arm_update_needed = true;
}
// Arm gripper rotate (up arrow and down arrow)
if (keys[keyboard::Key::KEY_UP]) {
// OPEN GRIPPER
if (arm_servo[ARM_GRIPPER_CLAW] - GRIPPER_CLAW_INCREMENT <= GRIPPER_CLAW_MIN) {
arm_servo[ARM_GRIPPER_CLAW] = GRIPPER_CLAW_MIN;
} else {
arm_servo[ARM_GRIPPER_CLAW] -= GRIPPER_CLAW_INCREMENT;
}
arm_update_needed = true;
} else if (keys[keyboard::Key::KEY_DOWN]) {
// CLOSE GRIPPER
if (arm_servo[ARM_GRIPPER_CLAW] + GRIPPER_CLAW_INCREMENT >= GRIPPER_CLAW_MAX) {
arm_servo[ARM_GRIPPER_CLAW] = GRIPPER_CLAW_MAX;
} else {
arm_servo[ARM_GRIPPER_CLAW] += GRIPPER_CLAW_INCREMENT;
}
arm_update_needed = true;
}
// Steering (a and d and f)
// q = CCW; e = cw
if (keys[keyboard::Key::KEY_a]) {
steer_servo[STEER_BACK] += 3;
steer_servo[STEER_FRONT_RIGHT] += 3;
steer_servo[STEER_FRONT_LEFT] += 3;
steer_update_needed = true;
} else if (keys[keyboard::Key::KEY_d]) {
steer_servo[STEER_BACK] -= 3;
steer_servo[STEER_FRONT_RIGHT] -= 3;
steer_servo[STEER_FRONT_LEFT] -= 3;
steer_update_needed = true;
}
if (keys[keyboard::Key::KEY_f]) {
steer_servo[STEER_BACK] = 0;
steer_servo[STEER_FRONT_RIGHT] = 0;
steer_servo[STEER_FRONT_LEFT] = 0;
steer_update_needed = true;
}
// Drive motors (w and s and x)
if (keys[keyboard::Key::KEY_w]) {
(drive_motors[DRIVE_REAR] < 2000) ? drive_motors[DRIVE_REAR] += 100 : drive_motors[DRIVE_REAR] = 2000;
(drive_motors[DRIVE_SIDE_RIGHT] < 2000) ? drive_motors[DRIVE_SIDE_RIGHT] += 100 : drive_motors[DRIVE_SIDE_RIGHT] = 2000;
(drive_motors[DRIVE_SIDE_LEFT] < 2000) ? drive_motors[DRIVE_SIDE_LEFT] += 100 : drive_motors[DRIVE_SIDE_LEFT] = 2000;
(drive_motors[DRIVE_FRONT_RIGHT] < 2000) ? drive_motors[DRIVE_FRONT_RIGHT] += 100 : drive_motors[DRIVE_FRONT_RIGHT] = 2000;
(drive_motors[DRIVE_FRONT_LEFT] < 2000) ? drive_motors[DRIVE_FRONT_LEFT] += 100 : drive_motors[DRIVE_FRONT_LEFT] = 2000;
drive_update_needed = true;
} else if (keys[keyboard::Key::KEY_s]) {
(drive_motors[DRIVE_REAR] > -2000) ? drive_motors[DRIVE_REAR] -= 100 : drive_motors[DRIVE_REAR] = -2000;
(drive_motors[DRIVE_SIDE_RIGHT] > -2000) ? drive_motors[DRIVE_SIDE_RIGHT] -= 100 : drive_motors[DRIVE_SIDE_RIGHT] = -2000;
(drive_motors[DRIVE_SIDE_LEFT] > -2000) ? drive_motors[DRIVE_SIDE_LEFT] -= 100 : drive_motors[DRIVE_SIDE_LEFT] = -2000;
(drive_motors[DRIVE_FRONT_LEFT] > -2000) ? drive_motors[DRIVE_FRONT_RIGHT] -= 100 : drive_motors[DRIVE_FRONT_RIGHT] = -2000;
(drive_motors[DRIVE_FRONT_LEFT] > -2000) ? drive_motors[DRIVE_FRONT_LEFT] -= 100 : drive_motors[DRIVE_FRONT_LEFT] = -2000;
drive_update_needed = true;
}
if (keys[keyboard::Key::KEY_x]) {
drive_motors[DRIVE_REAR] = 0;
drive_motors[DRIVE_SIDE_RIGHT] = 0;
drive_motors[DRIVE_SIDE_LEFT] = 0;
drive_motors[DRIVE_FRONT_RIGHT] = 0;
drive_motors[DRIVE_FRONT_LEFT] = 0;
drive_update_needed = true;
}
// Mast Servo (q & w)
if (keys[keyboard::Key::KEY_q]) {
mast_servo = 5;
if (0 < mast_release_timeout) {
// If the mast was not previously moving
mast_release_timeout = 0;
mast_update_needed = true;
}
} else if (keys[keyboard::Key::KEY_e]) {
mast_servo = -5;
if (0 < mast_release_timeout) {
// If the mast was not previously moving
mast_release_timeout = 0;
mast_update_needed = true;
}
} else if (mast_release_timeout < 5) {
// Only execute this code if mast was previously moving
mast_release_timeout++;
mast_servo = 0;
mast_update_needed = true;
}
// Check if comamnd updates are needed
if (arm_update_needed) {
arm_update_needed = false;
publish_arm_servo_update();
}
if (steer_update_needed) {
steer_update_needed = false;
publish_steer_servo_update();
}
if (drive_update_needed) {
drive_update_needed = false;
publish_drive_motor_update();
}
if (mast_update_needed) {
mast_update_needed = false;
publish_mast_servo_update();
}
//
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
