irobotActuator.cpp

00001 #include "irobotActuator.h"
00002 #include <stdlib.h>
00003 
00004 #ifndef MAX
00005     #define MAX(x, y) (((x) > (y)) ? (x) : (y))
00006 #endif
00007 #ifndef MIN
00008     #define MIN(x, y) (((x) < (y)) ? (x) : (y))
00009 #endif
00010 #ifndef coerce
00011     #define coerce(xMin, x, xMax)   (MIN(MAX(xMin, x), xMax))
00012 #endif
00013 
00014 int32_t irobotDigitalOutputs(
00015     const irobotUARTPort_t  port,
00016     const uint8_t           output
00017 ){
00018     uint8_t packet[OP_DIGITAL_OUTPUTS_SIZE];
00019     
00020     packet[0] = OP_DIGITAL_OUTPUTS;
00021     packet[1] = output;
00022 
00023     return irobotUARTWriteRaw(port, packet, OP_DIGITAL_OUTPUTS_SIZE);
00024 }
00025 
00026 int32_t irobotDriveDirect(
00027     const irobotUARTPort_t  port,
00028     int16_t                 leftWheelSpeed,
00029     int16_t                 rightWheelSpeed
00030 ){
00031     uint8_t packet[OP_DRIVE_DIRECT_SIZE];
00032     
00033     leftWheelSpeed = (int16_t)coerce(ACTUATOR_WHEEL_SPEED_MIN, leftWheelSpeed, ACTUATOR_WHEEL_SPEED_MAX);
00034     rightWheelSpeed = (int16_t)coerce(ACTUATOR_WHEEL_SPEED_MIN, rightWheelSpeed, ACTUATOR_WHEEL_SPEED_MAX);
00035 
00036     packet[0] = OP_DRIVE_DIRECT;
00037     packet[1] = HO(rightWheelSpeed);
00038     packet[2] = LO(rightWheelSpeed);
00039     packet[3] = HO(leftWheelSpeed);
00040     packet[4] = LO(leftWheelSpeed);
00041 
00042     return irobotUARTWriteRaw(port, packet, OP_DRIVE_DIRECT_SIZE);
00043 }
00044 
00045 int32_t irobotDriveDirection(
00046     const irobotUARTPort_t      port,
00047     int16_t                     velocity,
00048     const irobotDirection_t     direction
00049 ){
00050     uint8_t packet[OP_DRIVE_SIZE];
00051     
00052     velocity = (int16_t)coerce(ACTUATOR_WHEEL_SPEED_MIN, velocity, ACTUATOR_WHEEL_SPEED_MAX);
00053 
00054     packet[0] = OP_DRIVE;
00055     packet[1] = HO(velocity);
00056     packet[2] = LO(velocity);
00057     packet[3] = HO((uint16_t)direction);
00058     packet[4] = LO((uint16_t)direction);
00059 
00060     return irobotUARTWriteRaw(port, packet, OP_DRIVE_SIZE);
00061 }
00062 
00063 int32_t irobotDriveRadius(
00064     const irobotUARTPort_t  port,
00065     int16_t                 velocity,
00066     int16_t                 radius
00067 ){
00068     uint8_t packet[OP_DRIVE_SIZE];
00069 
00070     velocity = (int16_t)coerce(ACTUATOR_WHEEL_SPEED_MIN, velocity, ACTUATOR_WHEEL_SPEED_MAX);
00071     radius = (int16_t)coerce(ACTUATOR_DRIVE_RADIUS_MIN, radius, ACTUATOR_DRIVE_RADIUS_MAX);
00072 
00073     // Special case: radius = 1mm is interpreted as CCW rotation;
00074     //  iRobot Drive CCW covers this case, so a drive radius of 1mm is
00075     //  interpreted as the next smallest radius, 2mm
00076     if(radius == 1){
00077         radius = 2;
00078     }
00079 
00080     packet[0] = OP_DRIVE;
00081     packet[1] = HO(velocity);
00082     packet[2] = LO(velocity);
00083     packet[3] = HO(radius);
00084     packet[4] = LO(radius);
00085     
00086     return irobotUARTWriteRaw(port, packet, OP_DRIVE_SIZE);
00087 }
00088 
00089 int32_t irobotLEDs(
00090     const irobotUARTPort_t  port,
00091     const irobotLED_t       leds,
00092     const uint8_t           powerColor,
00093     const uint8_t           powerIntensity
00094 ){
00095     uint8_t packet[OP_LEDS_SIZE];
00096 
00097     packet[0] = OP_LEDS;
00098     packet[1] = (uint8_t)leds;
00099     packet[2] = powerColor;
00100     packet[3] = powerIntensity;
00101     
00102     return irobotUARTWriteRaw(port, packet, OP_LEDS_SIZE);
00103 }
00104 
00105 int32_t irobotPWMLowSideDrivers(
00106     const irobotUARTPort_t  port,
00107     uint8_t                 pwm0,
00108     uint8_t                 pwm1,
00109     uint8_t                 pwm2
00110 ){
00111     uint8_t packet[OP_PWM_LOW_SIDE_DRIVERS_SIZE];
00112 
00113     pwm0 = (uint16_t)coerce(ACTUATOR_PWM_DUTY_MIN, pwm0, ACTUATOR_PWM_DUTY_MAX);
00114     pwm1 = (uint16_t)coerce(ACTUATOR_PWM_DUTY_MIN, pwm1, ACTUATOR_PWM_DUTY_MAX);
00115     pwm2 = (uint16_t)coerce(ACTUATOR_PWM_DUTY_MIN, pwm2, ACTUATOR_PWM_DUTY_MAX);
00116 
00117     packet[0] = OP_PWM_LOW_SIDE_DRIVERS;
00118     packet[1] = pwm0;
00119     packet[2] = pwm1;
00120     packet[3] = pwm2;
00121     
00122     return irobotUARTWriteRaw(port, packet, OP_PWM_LOW_SIDE_DRIVERS_SIZE);
00123 }
00124 
00125 /// opcode + song number + song length + n notes
00126 #define OP_SONG_MAX_LENGTH  (ACTUATOR_MAX_SONGS * ACTUATOR_MAX_NOTES_PER_SONG + 3)
00127 
00128 int32_t irobotSong(
00129     const irobotUARTPort_t          port,
00130     uint8_t                         songNumber,
00131     const irobotSongNote_t * const  songNotes,
00132     uint8_t                         nNotes
00133 ){
00134     uint8_t packet[OP_SONG_MAX_LENGTH];
00135     uint8_t packetIndex = 0;
00136     uint8_t noteIndex = 0;
00137 
00138     if(!songNotes){
00139         return ERROR_INVALID_PARAMETER;
00140     }
00141 
00142     songNumber = MIN(songNumber, ACTUATOR_MAX_SONGS - 1);
00143     nNotes = MIN(nNotes, (ACTUATOR_MAX_SONGS - songNumber) * ACTUATOR_MAX_NOTES_PER_SONG);
00144 
00145     packet[packetIndex++] = OP_SONG;
00146     packet[packetIndex++] = songNumber;
00147     packet[packetIndex++] = nNotes;
00148     for(noteIndex = 0; noteIndex < nNotes; noteIndex++){
00149         packet[packetIndex++] = songNotes[noteIndex].midiNote;
00150         packet[packetIndex++] = songNotes[noteIndex].duration;
00151     }
00152     
00153     return irobotUARTWriteRaw(port, packet, packetIndex);
00154 }
00155 
00156 int32_t irobotPlaySong(
00157     const irobotUARTPort_t  port,
00158     const uint8_t           songNumber
00159 ){
00160     uint8_t packet[OP_PLAY_SONG_SIZE];
00161 
00162     packet[0] = OP_PLAY_SONG;
00163     packet[1] = songNumber;
00164     
00165     return irobotUARTWriteRaw(port, packet, OP_PLAY_SONG_SIZE);
00166 }
00167 
00168 int32_t irobotStop(
00169     const irobotUARTPort_t  port
00170 ){
00171     return irobotDriveDirect(port, 0, 0);
00172 }