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

/media/uploads/abotics/apeiros_gripper_10-04-2014.jpg

Apeiros is a low cost, flexible and entirely open source, educational mobile robot that has been designed for everyone by Abe Howell's Robotics, http://www.abotics.com. With an optional gripper mechanism Apeiros can manipulate objects and perform more advanced robotic tasks and experiments. Sensory expansion has been designed into this robot, so that you can add sensors as they are needed. I created Apeiros so that you can get hit the ground running in the wonderful world of robotics!

My Apeiros class library is released as open source under General Public License (GPL) Version 3, http://www.gnu.org/licenses/gpl-3.0.html, so please feel free to use and modify however you see fit.

Check out my Apeiros page for more information.

Below is an example main.cpp file that uses the Apeiros class library.

The Apeiros constructor passes a Serial transmit, tx, and receive, rx, PinName along with left and right motor PWM offsets. I am designing my wireless shield to use PA_9 for tx and PA_10 for rx. For now simply use the built-in USB serial connection by specifying SERIAL_TX & SERIAL_RX. No two gear motors will begin to rotate at the same PWM level, so I have allowed for the specification of a leftMotorPwmOffset and a rightMotorPwmOffset parameter. These values correspond to the PWM level that starts the respective motor rotating. You can determine these values by simply passing zero for the left and right PWM offsets. Then connect with Tera Term and issue the open-loop move command, MOxxx,yyy\r, where xxx is left motor speed and yyy is right motor speed (MO120,115\r). Simply vary the left and right motor speeds until you find the level at which each motor begins to rotate.These values will be the PWM motor offsets that need to be passed to the Apeiros constructor.

main.cpp

#include "mbed.h"
#include "Apeiros.h"

Apeiros apeiros(SERIAL_TX, SERIAL_RX,120,115);

int main() {
    
    apeiros.Begin();
    
    while(1) {
        
        if (apeiros.IsSerialDataAvailable()) apeiros.ParseUartData();
        
        // Add your custom code below//
                
    }
}

Below is a simple obstacle avoidance program that uses sensory input from a front left and front right IR sensor. The front left sensor's OUT pin must be connected to PC_15 and the front right sensor's OUT pin to PC_13 on the Nucleo F401RE's Morpho header.

main.cpp

#include "mbed.h"
#include "Apeiros.h"

Apeiros apeiros(SERIAL_TX, SERIAL_RX,100,100);

int main() {    
    apeiros.Begin();
    while(1) {
        int sensorState = 2*apeiros.leftFrontIR.read();
        sensorState += apeiros.rightFrontIR.read();
        
        switch (sensorState) {
            case 0:
              apeiros.SetMotorSpeed(-65,-65);
              apeiros.SetBuzzerTone(5);
              break;              
            case 1:
              apeiros.SetMotorSpeed(65,-65);
              apeiros.SetBuzzerTone(50);
              break;
            case 2:
              apeiros.SetMotorSpeed(-65,65);
              apeiros.SetBuzzerTone(30);
              break;              
            case 3:
              apeiros.SetMotorSpeed(65,65);
              apeiros.SetBuzzerTone(0);
              break;             
            default:
              apeiros.SetMotorSpeed(0,0);
              break;
        }        
        wait_ms(100);
    }
Committer:
abotics
Date:
Tue Dec 23 23:29:00 2014 +0000
Revision:
3:c61c3feb0729
Parent:
2:71ede4f26f71
Changed MAX_GRIPPER_PULSE from 2300 to 2200 to ensure that grippers do not interfere with wheels.; ; Added additional explanation regarding valid motor speed range.; ; Added DigitalIn objects in support for leftRearIR and rightRearIR sensors.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
abotics 0:72b953cdcfd1 1 /** Apeiros Robot class.
abotics 0:72b953cdcfd1 2 * Used for developing programs for the Apeiros Robot by Abe Howell's Robotics.
abotics 0:72b953cdcfd1 3 *
abotics 0:72b953cdcfd1 4 * Example:
abotics 0:72b953cdcfd1 5 * @code
abotics 0:72b953cdcfd1 6 * #include "mbed.h"
abotics 0:72b953cdcfd1 7 * #include "Apeiros.h"
abotics 0:72b953cdcfd1 8 *
abotics 3:c61c3feb0729 9 * Apeiros apeiros(SERIAL_TX, SERIAL_RX, 100, 100);
abotics 0:72b953cdcfd1 10 *
abotics 0:72b953cdcfd1 11 * int main() {
abotics 0:72b953cdcfd1 12 * apeiros.Begin();
abotics 1:8edccfd4646a 13 * while(1)
abotics 1:8edccfd4646a 14 * {
abotics 1:8edccfd4646a 15 * if (apeiros.IsSerialDataAvailable()) apeiros.ParseUartData();
abotics 1:8edccfd4646a 16 * }
abotics 0:72b953cdcfd1 17 * }
abotics 0:72b953cdcfd1 18 * @endcode
abotics 0:72b953cdcfd1 19 */
abotics 0:72b953cdcfd1 20
abotics 0:72b953cdcfd1 21 #ifndef MBED_APEIROS_H
abotics 0:72b953cdcfd1 22 #define MBED_APEIROS_H
abotics 0:72b953cdcfd1 23
abotics 0:72b953cdcfd1 24 #include "mbed.h"
abotics 0:72b953cdcfd1 25
abotics 0:72b953cdcfd1 26 #define Dwh 2.621 // wheel diameter
abotics 0:72b953cdcfd1 27 #define PI 3.14159 // PI
abotics 0:72b953cdcfd1 28 #define Cwh (Dwh * PI) // wheel circumference
abotics 0:72b953cdcfd1 29 #define TSCALE 10 // convert to 10ths of an inch
abotics 0:72b953cdcfd1 30 #define INVTICK 4883 // this is 1 / 204.8 us, to avoid using floating point math
abotics 0:72b953cdcfd1 31 #define NCLKS 128 // number of encoder clocks per wheel rotation
abotics 0:72b953cdcfd1 32 #define Ktps ((Cwh * TSCALE * INVTICK) / NCLKS)
abotics 0:72b953cdcfd1 33 #define MaxMotorSpeed 255
abotics 0:72b953cdcfd1 34
abotics 3:c61c3feb0729 35 // Hitec HS-81 accepts a 600 to 2400us pulse control signal. //
abotics 3:c61c3feb0729 36 // Do not use a value that is outside of the HS-81 specification! //
abotics 0:72b953cdcfd1 37 #define MIN_GRIPPER_PULSE 1000
abotics 3:c61c3feb0729 38 #define MAX_GRIPPER_PULSE 2200
abotics 0:72b953cdcfd1 39
abotics 0:72b953cdcfd1 40 #define MAX_BUZZER_PWM 100
abotics 2:71ede4f26f71 41
abotics 2:71ede4f26f71 42 /** Create Apeiros class
abotics 2:71ede4f26f71 43 *
abotics 2:71ede4f26f71 44 * @param Apeiros class
abotics 2:71ede4f26f71 45 *
abotics 2:71ede4f26f71 46 */
abotics 0:72b953cdcfd1 47
abotics 0:72b953cdcfd1 48 class Apeiros : public Serial{
abotics 0:72b953cdcfd1 49
abotics 0:72b953cdcfd1 50 public:
abotics 0:72b953cdcfd1 51
abotics 0:72b953cdcfd1 52 /** Create Apeiros instance
abotics 0:72b953cdcfd1 53 *
abotics 3:c61c3feb0729 54 * @param tx specified as PinName of UART Tx pin. Use SERIAL_TX as default. Use PA_9 for wireless shield.
abotics 3:c61c3feb0729 55 * @param rx specified as PinName of UART Rx pin. Use SERIAL_RX as deafult. Use PA_10 for wireless shield.
abotics 3:c61c3feb0729 56 * @param leftMotorPwmOffset specified as integer value from 0-150. Use 100 as default.
abotics 3:c61c3feb0729 57 * @param rightMotorPwmOffset specified as integer value from 0-150. Use 100 as default.
abotics 3:c61c3feb0729 58 *
abotics 3:c61c3feb0729 59 * The PwmOffsets are used to eliminate the deadband for both the left & right motor, so
abotics 3:c61c3feb0729 60 * that each motor begins to rotate when motor speeds are specified as aperiros.SetMotorSpeed(1,1).
abotics 3:c61c3feb0729 61 *
abotics 3:c61c3feb0729 62 * Pass a zero for each PwmOffset parameter and then use SetMotorSpeed function to find the leftMotorPwmOffset & rightMotorPwmOffset values.
abotics 3:c61c3feb0729 63 *
abotics 3:c61c3feb0729 64 * You have found the PwmOffset value when the respective motor goes from not rotating to starting rotation.
abotics 0:72b953cdcfd1 65 */
abotics 0:72b953cdcfd1 66 Apeiros(PinName tx, PinName rx, int leftMotorPwmOffset, int rightMotorPwmOffset);
abotics 0:72b953cdcfd1 67
abotics 0:72b953cdcfd1 68 /** Begin using Apeiros class. Must be called before using class functions.
abotics 0:72b953cdcfd1 69 *
abotics 0:72b953cdcfd1 70 */
abotics 0:72b953cdcfd1 71 void Begin(void);
abotics 0:72b953cdcfd1 72
abotics 0:72b953cdcfd1 73 /** Is Serial data available to be parsed.
abotics 0:72b953cdcfd1 74 *
abotics 0:72b953cdcfd1 75 * @returns
abotics 0:72b953cdcfd1 76 * 1 if serial data is available,
abotics 0:72b953cdcfd1 77 * 0 if no serial data has been received
abotics 0:72b953cdcfd1 78 */
abotics 0:72b953cdcfd1 79 bool IsSerialDataAvailable(void);
abotics 0:72b953cdcfd1 80
abotics 0:72b953cdcfd1 81 /** Parse available serial UART data.
abotics 0:72b953cdcfd1 82 *
abotics 0:72b953cdcfd1 83 */
abotics 0:72b953cdcfd1 84 void ParseUartData(void);
abotics 0:72b953cdcfd1 85
abotics 0:72b953cdcfd1 86 /** Set piezo buzzer tone.
abotics 0:72b953cdcfd1 87 *
abotics 0:72b953cdcfd1 88 * @param buzzerTone specified as an integer value(int) from 0-100.
abotics 0:72b953cdcfd1 89 */
abotics 0:72b953cdcfd1 90 void SetBuzzerTone(int buzzerTone);
abotics 0:72b953cdcfd1 91
abotics 3:c61c3feb0729 92 /** Set left and right motor speeds.
abotics 3:c61c3feb0729 93 *
abotics 3:c61c3feb0729 94 * @param leftMotorSpeed specified as an integer value(int) from 0 to (255-leftMotorPwmOffset). Use a preceeding negative sign to reverse motor rotation.
abotics 3:c61c3feb0729 95 * @param rightMotorSpeed specified as an integer value(int) from 0 to (255-rightMotorPwmOffset). Use a preceeding negative sign to reverse motor rotation.
abotics 3:c61c3feb0729 96 *
abotics 3:c61c3feb0729 97 * For example, if leftMotorPwmOffset = 95 then valid left motor speed range is 0 to 165. Use a preceeding negative sign to reverse motor rotation.
abotics 0:72b953cdcfd1 98 *
abotics 3:c61c3feb0729 99 * Drive robot forward with apeiros.SetMotorSpeed(65,65).
abotics 3:c61c3feb0729 100 *
abotics 3:c61c3feb0729 101 * Drive robot in reverse with apeiros.SetMotorSpeed(-65,-65).
abotics 3:c61c3feb0729 102 *
abotics 3:c61c3feb0729 103 * Spin robot clockwise with apeiros.SetMotorSpeed(65,-65).
abotics 3:c61c3feb0729 104 *
abotics 3:c61c3feb0729 105 * Spin robot counterclockwise with apeiros.SetMotorSpeed(-65,65).
abotics 0:72b953cdcfd1 106 */
abotics 0:72b953cdcfd1 107 void SetMotorSpeed(int leftMotorSpeed, int rightMotorSpeed);
abotics 0:72b953cdcfd1 108
abotics 0:72b953cdcfd1 109 /** Set servo gripper position.
abotics 0:72b953cdcfd1 110 *
abotics 3:c61c3feb0729 111 * @param pulseWidth_us specified as an integer value(int) from MIN_GRIPPER_PULSE(1000) to MAX_GRIPPER_PULSE(2200).
abotics 0:72b953cdcfd1 112 */
abotics 0:72b953cdcfd1 113 void SetGripperPosition(int pulseWidth_us);
abotics 0:72b953cdcfd1 114
abotics 1:8edccfd4646a 115 /** Return integer count for left wheel encoder.
abotics 1:8edccfd4646a 116 *
abotics 1:8edccfd4646a 117 * @returns
abotics 1:8edccfd4646a 118 * Integer value for left wheel encoder count.
abotics 1:8edccfd4646a 119 */
abotics 1:8edccfd4646a 120 int GetLeftEncoder(void);
abotics 1:8edccfd4646a 121
abotics 1:8edccfd4646a 122 /** Return integer count for right wheel encoder.
abotics 1:8edccfd4646a 123 *
abotics 1:8edccfd4646a 124 * @returns
abotics 1:8edccfd4646a 125 * Integer value for right wheel encoder count.
abotics 1:8edccfd4646a 126 */
abotics 1:8edccfd4646a 127 int GetRightEncoder(void);
abotics 1:8edccfd4646a 128
abotics 1:8edccfd4646a 129 /** Reset both left & right wheel encoder counts.
abotics 1:8edccfd4646a 130 *
abotics 1:8edccfd4646a 131 */
abotics 1:8edccfd4646a 132 void ResetWheelEncoders(void);
abotics 1:8edccfd4646a 133
abotics 3:c61c3feb0729 134 /** DigitalIn object for Left Front IR Sensor.
abotics 3:c61c3feb0729 135 *
abotics 3:c61c3feb0729 136 */
abotics 0:72b953cdcfd1 137 DigitalIn leftFrontIR;
abotics 3:c61c3feb0729 138
abotics 3:c61c3feb0729 139 /** DigitalIn object for Center Front IR Sensor.
abotics 3:c61c3feb0729 140 *
abotics 3:c61c3feb0729 141 */
abotics 0:72b953cdcfd1 142 DigitalIn centerFrontIR;
abotics 3:c61c3feb0729 143
abotics 3:c61c3feb0729 144 /** DigitalIn object for Right Front IR Sensor.
abotics 3:c61c3feb0729 145 *
abotics 3:c61c3feb0729 146 */
abotics 0:72b953cdcfd1 147 DigitalIn rightFrontIR;
abotics 0:72b953cdcfd1 148
abotics 3:c61c3feb0729 149 /** DigitalIn object for Left Rear IR Sensor.
abotics 3:c61c3feb0729 150 *
abotics 3:c61c3feb0729 151 */
abotics 3:c61c3feb0729 152 DigitalIn leftRearIR;
abotics 3:c61c3feb0729 153
abotics 3:c61c3feb0729 154 /** DigitalIn object for Right Rear IR Sensor.
abotics 3:c61c3feb0729 155 *
abotics 3:c61c3feb0729 156 */
abotics 3:c61c3feb0729 157 DigitalIn rightRearIR;
abotics 3:c61c3feb0729 158
abotics 3:c61c3feb0729 159 /** ad_0 AnalogIn object. Used to convert a varying voltage to a digital value.
abotics 3:c61c3feb0729 160 *
abotics 3:c61c3feb0729 161 */
abotics 0:72b953cdcfd1 162 AnalogIn ad_0;
abotics 3:c61c3feb0729 163 /** ad_1 AnalogIn object. Used to convert a varying voltage to a digital value.
abotics 3:c61c3feb0729 164 *
abotics 3:c61c3feb0729 165 */
abotics 0:72b953cdcfd1 166 AnalogIn ad_1;
abotics 3:c61c3feb0729 167 /** ad_2 AnalogIn object. Used to convert a varying voltage to a digital value.
abotics 3:c61c3feb0729 168 *
abotics 3:c61c3feb0729 169 */
abotics 0:72b953cdcfd1 170 AnalogIn ad_2;
abotics 3:c61c3feb0729 171 /** ad_3 AnalogIn object. Used to convert a varying voltage to a digital value.
abotics 3:c61c3feb0729 172 *
abotics 3:c61c3feb0729 173 */
abotics 0:72b953cdcfd1 174 AnalogIn ad_3;
abotics 3:c61c3feb0729 175 /** ad_4 AnalogIn object. Used to convert a varying voltage to a digital value.
abotics 3:c61c3feb0729 176 *
abotics 3:c61c3feb0729 177 */
abotics 0:72b953cdcfd1 178 AnalogIn ad_4;
abotics 3:c61c3feb0729 179 /** ad_5 AnalogIn object. Used to convert a varying voltage to a digital value.
abotics 3:c61c3feb0729 180 *
abotics 3:c61c3feb0729 181 */
abotics 0:72b953cdcfd1 182 AnalogIn ad_5;
abotics 0:72b953cdcfd1 183
abotics 1:8edccfd4646a 184
abotics 1:8edccfd4646a 185
abotics 0:72b953cdcfd1 186 private:
abotics 0:72b953cdcfd1 187
abotics 0:72b953cdcfd1 188 DigitalOut _buzzerOut;
abotics 0:72b953cdcfd1 189
abotics 0:72b953cdcfd1 190 DigitalOut _SN_3A;
abotics 0:72b953cdcfd1 191 DigitalOut _SN_4A;
abotics 0:72b953cdcfd1 192
abotics 0:72b953cdcfd1 193 DigitalOut _SN_2A;
abotics 0:72b953cdcfd1 194 DigitalOut _SN_1A;
abotics 0:72b953cdcfd1 195
abotics 0:72b953cdcfd1 196 DigitalIn _leftEncoderDirPin;
abotics 0:72b953cdcfd1 197 DigitalIn _rightEncoderDirPin;
abotics 0:72b953cdcfd1 198
abotics 0:72b953cdcfd1 199 InterruptIn _leftEncoderClk;
abotics 0:72b953cdcfd1 200 InterruptIn _rightEncoderClk;
abotics 0:72b953cdcfd1 201
abotics 0:72b953cdcfd1 202 PwmOut _rightMotorPWM;
abotics 0:72b953cdcfd1 203 PwmOut _leftMotorPWM;
abotics 0:72b953cdcfd1 204 PwmOut _gripperPWM;
abotics 0:72b953cdcfd1 205
abotics 0:72b953cdcfd1 206 Ticker _motorControl;
abotics 0:72b953cdcfd1 207
abotics 0:72b953cdcfd1 208 volatile char rxBuff[16];
abotics 0:72b953cdcfd1 209 volatile char rxBuffIndx;
abotics 0:72b953cdcfd1 210 volatile char tmpRxBuff[16];
abotics 0:72b953cdcfd1 211 volatile bool uartDataRdy;
abotics 0:72b953cdcfd1 212
abotics 0:72b953cdcfd1 213 volatile int motorUpdateTickCount;
abotics 0:72b953cdcfd1 214 volatile int motionLoopCount;
abotics 0:72b953cdcfd1 215
abotics 0:72b953cdcfd1 216 volatile int leftEncoderCount;
abotics 0:72b953cdcfd1 217 volatile int rightEncoderCount;
abotics 0:72b953cdcfd1 218 volatile bool leftEncoderDir;
abotics 0:72b953cdcfd1 219 volatile bool rightEncoderDir;
abotics 0:72b953cdcfd1 220 volatile int leftMotorOffset;
abotics 0:72b953cdcfd1 221 volatile int rightMotorOffset;
abotics 0:72b953cdcfd1 222
abotics 0:72b953cdcfd1 223 volatile unsigned short analogIndex;
abotics 0:72b953cdcfd1 224 volatile float adSensors[6];
abotics 0:72b953cdcfd1 225 volatile int analogUpdateCount;
abotics 0:72b953cdcfd1 226
abotics 0:72b953cdcfd1 227 volatile char encoderArrayIndex_L, encoderArrayIndex_R;
abotics 0:72b953cdcfd1 228 volatile int encoderPeriodArray_L[4], encoderPeriodArray_R[4];
abotics 0:72b953cdcfd1 229 volatile int encoderPeriod_L, encoderPeriod_R, encoderPeriodSum_L, encoderPeriodSum_R;
abotics 0:72b953cdcfd1 230 volatile int prevT3Count_L, prevT3Count_R;
abotics 0:72b953cdcfd1 231 volatile bool encoderUpdated_L, encoderUpdated_R;
abotics 0:72b953cdcfd1 232 volatile int encoderSpeed_L, encoderSpeed_R;
abotics 0:72b953cdcfd1 233
abotics 0:72b953cdcfd1 234 // Buzzer Variables //
abotics 0:72b953cdcfd1 235 volatile int buzzerPeriod;
abotics 0:72b953cdcfd1 236 volatile int buzzerDuty;
abotics 0:72b953cdcfd1 237 volatile int buzzerTick;
abotics 0:72b953cdcfd1 238 volatile bool enableBuzzer;
abotics 0:72b953cdcfd1 239
abotics 0:72b953cdcfd1 240 void getUartData(void);
abotics 0:72b953cdcfd1 241 void motorControlISR(void);
abotics 0:72b953cdcfd1 242 void leftEncoderTick(void);
abotics 0:72b953cdcfd1 243 void rightEncoderTick(void);
abotics 0:72b953cdcfd1 244
abotics 0:72b953cdcfd1 245 void ParseMotorCommand(void);
abotics 0:72b953cdcfd1 246 void ParseBuzzerCommand(void);
abotics 0:72b953cdcfd1 247 void ParseGripperCommand(void);
abotics 0:72b953cdcfd1 248 void CalculateWheelSpeed(void);
abotics 0:72b953cdcfd1 249 void Initialize(void);
abotics 0:72b953cdcfd1 250
abotics 0:72b953cdcfd1 251 };
abotics 0:72b953cdcfd1 252
abotics 0:72b953cdcfd1 253 #endif