This class library has been created for use with the Apeiros Robot by Abe Howell's Robotics.

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Apeiros.h Source File

Apeiros.h

00001 /** Apeiros Robot class.
00002  *  Used for developing programs for the Apeiros Robot by Abe Howell's Robotics.
00003  *
00004  * Example:
00005  * @code
00006  * #include "mbed.h"
00007  * #include "Apeiros.h"
00008  *
00009  * Apeiros apeiros(SERIAL_TX, SERIAL_RX, 100, 100);
00010  * 
00011  * int main() {
00012  *     apeiros.Begin();
00013  *      while(1)
00014  *     {
00015  *        if (apeiros.IsSerialDataAvailable()) apeiros.ParseUartData();
00016  *     }
00017  * }
00018  * @endcode
00019  */
00020 
00021 #ifndef MBED_APEIROS_H
00022 #define MBED_APEIROS_H
00023  
00024 #include "mbed.h"
00025  
00026 #define Dwh 2.621       // wheel diameter
00027 #define PI 3.14159      // PI
00028 #define Cwh (Dwh * PI)  // wheel circumference
00029 #define TSCALE 10       // convert to 10ths of an inch
00030 #define INVTICK 4883    // this is 1 / 204.8 us, to avoid using floating point math
00031 #define NCLKS 128       // number of encoder clocks per wheel rotation
00032 #define Ktps ((Cwh * TSCALE * INVTICK) / NCLKS)
00033 #define MaxMotorSpeed 255
00034 
00035 // Hitec HS-81 accepts a 600 to 2400us pulse control signal. //
00036 // Do not use a value that is outside of the HS-81 specification! //
00037 #define MIN_GRIPPER_PULSE 1000
00038 #define MAX_GRIPPER_PULSE 2200
00039 
00040 #define MAX_BUZZER_PWM 100
00041 
00042 /** Create Apeiros class
00043      *
00044      * @param Apeiros class
00045      *
00046      */
00047  
00048 class Apeiros : public Serial{
00049 
00050 public:
00051 
00052     /** Create Apeiros instance
00053     *
00054     * @param tx specified as PinName of UART Tx pin. Use SERIAL_TX as default. Use PA_9 for wireless shield.
00055     * @param rx specified as PinName of UART Rx pin. Use SERIAL_RX as deafult. Use PA_10 for wireless shield.
00056     * @param leftMotorPwmOffset specified as integer value from 0-150. Use 100 as default.
00057     * @param rightMotorPwmOffset specified as integer value from 0-150. Use 100 as default.
00058     * 
00059     * The PwmOffsets are used to eliminate the deadband for both the left & right motor, so
00060     * that each motor begins to rotate when motor speeds are specified as aperiros.SetMotorSpeed(1,1).
00061     *
00062     * Pass a zero for each PwmOffset parameter and then use SetMotorSpeed function to find the leftMotorPwmOffset & rightMotorPwmOffset values.
00063     *
00064     * You have found the PwmOffset value when the respective motor goes from not rotating to starting rotation.
00065     */
00066     Apeiros(PinName tx, PinName rx, int leftMotorPwmOffset, int rightMotorPwmOffset);
00067     
00068     /** Begin using Apeiros class. Must be called before using class functions.
00069      *
00070      */
00071     void Begin(void);
00072     
00073     /** Is Serial data available to be parsed.
00074      *
00075      * @returns
00076      *   1 if serial data is available,
00077      *   0 if no serial data has been received
00078      */
00079     bool IsSerialDataAvailable(void);
00080     
00081     /** Parse available serial UART data.
00082      *
00083      */
00084     void ParseUartData(void);
00085     
00086     /** Set piezo buzzer tone.
00087      *
00088      * @param buzzerTone specified as an integer value(int) from 0-100.
00089      */
00090     void SetBuzzerTone(int buzzerTone);
00091     
00092     /** Set left and right motor speeds.
00093      *
00094      * @param leftMotorSpeed specified as an integer value(int) from 0 to (255-leftMotorPwmOffset). Use a preceeding negative sign to reverse motor rotation.
00095      * @param rightMotorSpeed specified as an integer value(int) from 0 to (255-rightMotorPwmOffset). Use a preceeding negative sign to reverse motor rotation.
00096      * 
00097      * For example, if leftMotorPwmOffset = 95 then valid left motor speed range is 0 to 165. Use a preceeding negative sign to reverse motor rotation.
00098      *
00099      * Drive robot forward with apeiros.SetMotorSpeed(65,65).
00100      *
00101      * Drive robot in reverse with apeiros.SetMotorSpeed(-65,-65).
00102      *
00103      * Spin robot clockwise with apeiros.SetMotorSpeed(65,-65).
00104      *
00105      * Spin robot counterclockwise with apeiros.SetMotorSpeed(-65,65).
00106      */
00107     void SetMotorSpeed(int leftMotorSpeed, int rightMotorSpeed);
00108     
00109     /** Set servo gripper position.
00110      *
00111      * @param pulseWidth_us specified as an integer value(int) from MIN_GRIPPER_PULSE(1000) to MAX_GRIPPER_PULSE(2200).
00112      */
00113     void SetGripperPosition(int pulseWidth_us);
00114     
00115     /** Return integer count for left wheel encoder.
00116      *
00117      * @returns
00118      *   Integer value for left wheel encoder count.
00119      */
00120     int GetLeftEncoder(void);
00121     
00122     /** Return integer count for right wheel encoder.
00123      *
00124      * @returns
00125      *   Integer value for right wheel encoder count.
00126      */
00127     int GetRightEncoder(void);
00128     
00129     /** Reset both left & right wheel encoder counts.
00130      *
00131      */
00132     void ResetWheelEncoders(void);
00133     
00134     /** DigitalIn object for Left Front IR Sensor.
00135      *
00136      */
00137     DigitalIn leftFrontIR;
00138     
00139     /** DigitalIn object for Center Front IR Sensor.
00140      *
00141      */
00142     DigitalIn centerFrontIR;
00143     
00144     /** DigitalIn object for Right Front IR Sensor.
00145      *
00146      */
00147     DigitalIn rightFrontIR;
00148     
00149     /** DigitalIn object for Left Rear IR Sensor.
00150      *
00151      */
00152     DigitalIn leftRearIR;
00153     
00154     /** DigitalIn object for Right Rear IR Sensor.
00155      *
00156      */
00157     DigitalIn rightRearIR;
00158     
00159     /** ad_0 AnalogIn object. Used to convert a varying voltage to a digital value.
00160      *
00161      */
00162     AnalogIn ad_0;
00163     /** ad_1 AnalogIn object. Used to convert a varying voltage to a digital value.
00164      *
00165      */
00166     AnalogIn ad_1;
00167     /** ad_2 AnalogIn object. Used to convert a varying voltage to a digital value.
00168      *
00169      */
00170     AnalogIn ad_2;
00171     /** ad_3 AnalogIn object. Used to convert a varying voltage to a digital value.
00172      *
00173      */
00174     AnalogIn ad_3;
00175     /** ad_4 AnalogIn object. Used to convert a varying voltage to a digital value.
00176      *
00177      */
00178     AnalogIn ad_4;
00179     /** ad_5 AnalogIn object. Used to convert a varying voltage to a digital value.
00180      *
00181      */
00182     AnalogIn ad_5;
00183     
00184 
00185     
00186 private:
00187 
00188     DigitalOut _buzzerOut;
00189 
00190     DigitalOut _SN_3A;
00191     DigitalOut _SN_4A;
00192 
00193     DigitalOut _SN_2A;
00194     DigitalOut _SN_1A;
00195 
00196     DigitalIn _leftEncoderDirPin;
00197     DigitalIn _rightEncoderDirPin;
00198     
00199     InterruptIn _leftEncoderClk;
00200     InterruptIn _rightEncoderClk;
00201     
00202     PwmOut _rightMotorPWM;
00203     PwmOut _leftMotorPWM;
00204     PwmOut _gripperPWM;
00205     
00206     Ticker _motorControl;
00207     
00208     volatile char rxBuff[16];
00209     volatile char rxBuffIndx;
00210     volatile char tmpRxBuff[16];
00211     volatile bool uartDataRdy;
00212     
00213     volatile int motorUpdateTickCount;
00214     volatile int motionLoopCount;
00215     
00216     volatile int leftEncoderCount;
00217     volatile int rightEncoderCount;
00218     volatile bool leftEncoderDir;
00219     volatile bool rightEncoderDir;
00220     volatile int leftMotorOffset;
00221     volatile int rightMotorOffset;
00222     
00223     volatile unsigned short analogIndex;
00224     volatile float adSensors[6];
00225     volatile int analogUpdateCount;
00226     
00227     volatile char encoderArrayIndex_L, encoderArrayIndex_R;
00228     volatile int encoderPeriodArray_L[4], encoderPeriodArray_R[4];
00229     volatile int encoderPeriod_L, encoderPeriod_R, encoderPeriodSum_L, encoderPeriodSum_R;
00230     volatile int prevT3Count_L, prevT3Count_R;
00231     volatile bool encoderUpdated_L, encoderUpdated_R;
00232     volatile int encoderSpeed_L, encoderSpeed_R;
00233     
00234     // Buzzer Variables //
00235     volatile int buzzerPeriod;
00236     volatile int buzzerDuty;
00237     volatile int buzzerTick;
00238     volatile bool enableBuzzer;
00239 
00240     void getUartData(void);
00241     void motorControlISR(void);
00242     void leftEncoderTick(void);
00243     void rightEncoderTick(void);
00244     
00245     void ParseMotorCommand(void);
00246     void ParseBuzzerCommand(void);
00247     void ParseGripperCommand(void);
00248     void CalculateWheelSpeed(void);
00249     void Initialize(void);
00250 
00251 };
00252  
00253 #endif