void downArm(int armIndex) { int pwmSpeed = 20; if (armIndex == ARM_LEFT) { pwmServo(ARM_LEFT_PWM_INDEX, pwmSpeed, ARM_LEFT_DOWN); } else if (armIndex == ARM_RIGHT) { pwmServo(ARM_RIGHT_PWM_INDEX, pwmSpeed, ARM_RIGHT_DOWN); } else if (armIndex == 0) { pwmServo(ARM_LEFT_PWM_INDEX, pwmSpeed, ARM_LEFT_DOWN); pwmServo(ARM_RIGHT_PWM_INDEX, pwmSpeed, ARM_RIGHT_DOWN); } }
void deviceServoHandleRawData(unsigned char commandHeader, InputStream* inputStream, OutputStream* outputStream, OutputStream* notificationOutputStream) { // WRITE COMMANDS if (commandHeader == SERVO_COMMAND_WRITE) { ServoList* servoList = getServoDeviceServoList(); unsigned int servoIndex = readHex2(inputStream); checkIsSeparator(inputStream); unsigned int servoSpeed = readHex2(inputStream); checkIsSeparator(inputStream); unsigned int servoValue = readHex4(inputStream); if (servoIndex == SERVO_ALL_INDEX) { pwmServoAll(servoList, servoSpeed, servoValue); } else { Servo* servo = getServo(servoList, servoIndex); pwmServo(servo, servoSpeed, servoValue, false); } ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_WRITE); } else if (commandHeader == SERVO_COMMAND_WRITE_MAX_SPEED_UNDER_LOAD) { ServoList* servoList = getServoDeviceServoList(); unsigned int servoIndex = readHex2(inputStream); checkIsSeparator(inputStream); int servoMaxSpeedUnderLoad = readHex2(inputStream); Servo* servo = getServo(servoList, servoIndex); servo->maxSpeedUnderLoad = servoMaxSpeedUnderLoad; ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_WRITE_MAX_SPEED_UNDER_LOAD); } else if (commandHeader == SERVO_COMMAND_WRITE_COMPACT) { unsigned int servoValue = readHex4(inputStream); ServoList* servoList = getServoDeviceServoList(); pwmServoAll(servoList, PWM_SERVO_SPEED_MAX, servoValue); ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_WRITE_COMPACT); } // ENABLE / DISABLE if (commandHeader == SERVO_COMMAND_WRITE_ENABLE_DISABLE) { ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_WRITE_ENABLE_DISABLE); ServoList* servoList = getServoDeviceServoList(); unsigned int servoIndex = readHex2(inputStream); checkIsSeparator(inputStream); bool enabled = readBool(inputStream); if (servoIndex == SERVO_ALL_INDEX) { servoEnableAll(servoList, enabled); } else { Servo* servo = getServo(servoList, servoIndex); pwmServoSetEnabled(servo, enabled); } } else if (commandHeader == SERVO_COMMAND_ENABLE_DISABLE_ALL) { ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_ENABLE_DISABLE_ALL); ServoList* servoList = getServoDeviceServoList(); bool enabled = readBool(inputStream); servoEnableAll(servoList, enabled); } // READ COMMANDS else if (commandHeader == SERVO_COMMAND_GET_COUNT) { ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_GET_COUNT); ServoList* servoList = getServoDeviceServoList(); unsigned int servoCount = getServoCount(servoList); appendHex2(outputStream, servoCount); } else if (commandHeader == SERVO_COMMAND_READ) { ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_READ); unsigned int servoIndex = readHex2(inputStream); ServoList* servoList = getServoDeviceServoList(); Servo* servo = getServo(servoList, servoIndex); unsigned int speed = pwmServoReadTargetSpeed(servo); unsigned int currentPosition = pwmServoReadCurrentPosition(servo); unsigned int targetPosition = pwmServoReadTargetPosition(servo); appendHex2(outputStream, servoIndex); appendSeparator(outputStream); appendHex2(outputStream, speed); appendSeparator(outputStream); appendHex4(outputStream, currentPosition); appendSeparator(outputStream); appendHex4(outputStream, targetPosition); } else if (commandHeader == SERVO_COMMAND_READ_SPEED) { unsigned int servoIndex = readHex2(inputStream); ServoList* servoList = getServoDeviceServoList(); Servo* servo = getServo(servoList, servoIndex); unsigned int speed = pwmServoReadTargetSpeed(servo); ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_READ_SPEED); appendHex2(outputStream, speed); } else if (commandHeader == SERVO_COMMAND_READ_MAX_SPEED_UNDER_LOAD) { unsigned int servoIndex = readHex2(inputStream); ServoList* servoList = getServoDeviceServoList(); Servo* servo = getServo(servoList, servoIndex); unsigned int maxSpeedUnderLoad = pwmServoReadMaxSpeedUnderLoad(servo); ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_READ_MAX_SPEED_UNDER_LOAD); appendHex2(outputStream, maxSpeedUnderLoad); } else if (commandHeader == SERVO_COMMAND_READ_CURRENT_POSITION) { unsigned int servoIndex = readHex2(inputStream); ServoList* servoList = getServoDeviceServoList(); Servo* servo = getServo(servoList, servoIndex); int currentPosition = pwmServoReadCurrentPosition(servo); ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_READ_CURRENT_POSITION); appendHex4(outputStream, currentPosition); } else if (commandHeader == SERVO_COMMAND_READ_TARGET_POSITION) { unsigned int servoIndex = readHex2(inputStream); ServoList* servoList = getServoDeviceServoList(); Servo* servo = getServo(servoList, servoIndex); int targetPosition = pwmServoReadTargetPosition(servo); ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_READ_TARGET_POSITION); appendHex4(outputStream, targetPosition); } // DEBUG COMMANDS else if (commandHeader == SERVO_COMMAND_TEST) { ServoList* servoList = getServoDeviceServoList(); testAllPwmServos(servoList); ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_TEST); } else if (commandHeader == SERVO_COMMAND_GET_TIME_TO_REACH_UNDER_LOAD) { unsigned int servoIndex = readHex2(inputStream); checkIsSeparator(inputStream); unsigned int servoTargetPosition = readHex4(inputStream); ServoList* servoList = getServoDeviceServoList(); Servo* servo = getServo(servoList, servoIndex); unsigned int timeToReachUnderLoad = pwmServoComputeTimeMilliSecondsToReachTargetPosition(servo, servoTargetPosition); ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_GET_TIME_TO_REACH_UNDER_LOAD); appendHex4(outputStream, timeToReachUnderLoad); } else if (commandHeader == SERVO_COMMAND_DEBUG) { ServoList* servoList = getServoDeviceServoList(); printServoList(getInfoOutputStreamLogger(), servoList); ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_DEBUG); } }