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 16 17:54:27 2014 +0000
Revision:
2:71ede4f26f71
Parent:
1:8edccfd4646a
Child:
3:c61c3feb0729
Added auto-generated API documentation.

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 0:72b953cdcfd1 9 * Apeiros apeiros(SERIAL_TX, SERIAL_RX, 120, 120);
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 0:72b953cdcfd1 35 #define MIN_GRIPPER_PULSE 1000
abotics 0:72b953cdcfd1 36 #define MAX_GRIPPER_PULSE 2300
abotics 0:72b953cdcfd1 37
abotics 0:72b953cdcfd1 38 #define MAX_BUZZER_PWM 100
abotics 2:71ede4f26f71 39
abotics 2:71ede4f26f71 40 /** Create Apeiros class
abotics 2:71ede4f26f71 41 *
abotics 2:71ede4f26f71 42 * @param Apeiros class
abotics 2:71ede4f26f71 43 *
abotics 2:71ede4f26f71 44 */
abotics 0:72b953cdcfd1 45
abotics 0:72b953cdcfd1 46 class Apeiros : public Serial{
abotics 0:72b953cdcfd1 47
abotics 0:72b953cdcfd1 48 public:
abotics 0:72b953cdcfd1 49
abotics 0:72b953cdcfd1 50 /** Create Apeiros instance
abotics 0:72b953cdcfd1 51 *
abotics 0:72b953cdcfd1 52 * @param tx specified as PinName of UART Tx pin. Use SERIAL_TX as default.
abotics 0:72b953cdcfd1 53 * @param rx specified as PinName of UART Rx pin. Use SERIAL_RX as deafult.
abotics 0:72b953cdcfd1 54 * @param leftMotorPwmOffset specified as integer value from 0-150. Use 120 as default.
abotics 0:72b953cdcfd1 55 * @param rightMotorPwmOffset specified as integer value from 0-150. Use 120 as default.
abotics 0:72b953cdcfd1 56 */
abotics 0:72b953cdcfd1 57 Apeiros(PinName tx, PinName rx, int leftMotorPwmOffset, int rightMotorPwmOffset);
abotics 0:72b953cdcfd1 58
abotics 0:72b953cdcfd1 59 /** Begin using Apeiros class. Must be called before using class functions.
abotics 0:72b953cdcfd1 60 *
abotics 0:72b953cdcfd1 61 */
abotics 0:72b953cdcfd1 62 void Begin(void);
abotics 0:72b953cdcfd1 63
abotics 0:72b953cdcfd1 64 /** Is Serial data available to be parsed.
abotics 0:72b953cdcfd1 65 *
abotics 0:72b953cdcfd1 66 * @returns
abotics 0:72b953cdcfd1 67 * 1 if serial data is available,
abotics 0:72b953cdcfd1 68 * 0 if no serial data has been received
abotics 0:72b953cdcfd1 69 */
abotics 0:72b953cdcfd1 70 bool IsSerialDataAvailable(void);
abotics 0:72b953cdcfd1 71
abotics 0:72b953cdcfd1 72 /** Parse available serial UART data.
abotics 0:72b953cdcfd1 73 *
abotics 0:72b953cdcfd1 74 */
abotics 0:72b953cdcfd1 75 void ParseUartData(void);
abotics 0:72b953cdcfd1 76
abotics 0:72b953cdcfd1 77 /** Set piezo buzzer tone.
abotics 0:72b953cdcfd1 78 *
abotics 0:72b953cdcfd1 79 * @param buzzerTone specified as an integer value(int) from 0-100.
abotics 0:72b953cdcfd1 80 */
abotics 0:72b953cdcfd1 81 void SetBuzzerTone(int buzzerTone);
abotics 0:72b953cdcfd1 82
abotics 0:72b953cdcfd1 83 /** Set left and roght motor speeds.
abotics 0:72b953cdcfd1 84 *
abotics 0:72b953cdcfd1 85 * @param leftMotorSpeed specified as an integer value(int) from 0-150, rightMotorSpeed specified as an integer value(int) from 0-150.
abotics 0:72b953cdcfd1 86 */
abotics 0:72b953cdcfd1 87 void SetMotorSpeed(int leftMotorSpeed, int rightMotorSpeed);
abotics 0:72b953cdcfd1 88
abotics 0:72b953cdcfd1 89 /** Set servo gripper position.
abotics 0:72b953cdcfd1 90 *
abotics 0:72b953cdcfd1 91 * @param pulseWidth_us specified as an integer value(int) from MIN_GRIPPER_PULSE(1000) to MAX_GRIPPER_PULSE(2300).
abotics 0:72b953cdcfd1 92 */
abotics 0:72b953cdcfd1 93 void SetGripperPosition(int pulseWidth_us);
abotics 0:72b953cdcfd1 94
abotics 1:8edccfd4646a 95 /** Return integer count for left wheel encoder.
abotics 1:8edccfd4646a 96 *
abotics 1:8edccfd4646a 97 * @returns
abotics 1:8edccfd4646a 98 * Integer value for left wheel encoder count.
abotics 1:8edccfd4646a 99 */
abotics 1:8edccfd4646a 100 int GetLeftEncoder(void);
abotics 1:8edccfd4646a 101
abotics 1:8edccfd4646a 102 /** Return integer count for right wheel encoder.
abotics 1:8edccfd4646a 103 *
abotics 1:8edccfd4646a 104 * @returns
abotics 1:8edccfd4646a 105 * Integer value for right wheel encoder count.
abotics 1:8edccfd4646a 106 */
abotics 1:8edccfd4646a 107 int GetRightEncoder(void);
abotics 1:8edccfd4646a 108
abotics 1:8edccfd4646a 109 /** Reset both left & right wheel encoder counts.
abotics 1:8edccfd4646a 110 *
abotics 1:8edccfd4646a 111 */
abotics 1:8edccfd4646a 112 void ResetWheelEncoders(void);
abotics 1:8edccfd4646a 113
abotics 0:72b953cdcfd1 114 DigitalIn leftFrontIR;
abotics 0:72b953cdcfd1 115 DigitalIn centerFrontIR;
abotics 0:72b953cdcfd1 116 DigitalIn rightFrontIR;
abotics 0:72b953cdcfd1 117
abotics 0:72b953cdcfd1 118 AnalogIn ad_0;
abotics 0:72b953cdcfd1 119 AnalogIn ad_1;
abotics 0:72b953cdcfd1 120 AnalogIn ad_2;
abotics 0:72b953cdcfd1 121 AnalogIn ad_3;
abotics 0:72b953cdcfd1 122 AnalogIn ad_4;
abotics 0:72b953cdcfd1 123 AnalogIn ad_5;
abotics 0:72b953cdcfd1 124
abotics 1:8edccfd4646a 125
abotics 1:8edccfd4646a 126
abotics 0:72b953cdcfd1 127 private:
abotics 0:72b953cdcfd1 128
abotics 0:72b953cdcfd1 129 DigitalOut _buzzerOut;
abotics 0:72b953cdcfd1 130
abotics 0:72b953cdcfd1 131 DigitalOut _SN_3A;
abotics 0:72b953cdcfd1 132 DigitalOut _SN_4A;
abotics 0:72b953cdcfd1 133
abotics 0:72b953cdcfd1 134 DigitalOut _SN_2A;
abotics 0:72b953cdcfd1 135 DigitalOut _SN_1A;
abotics 0:72b953cdcfd1 136
abotics 0:72b953cdcfd1 137 DigitalIn _leftEncoderDirPin;
abotics 0:72b953cdcfd1 138 DigitalIn _rightEncoderDirPin;
abotics 0:72b953cdcfd1 139
abotics 0:72b953cdcfd1 140 InterruptIn _leftEncoderClk;
abotics 0:72b953cdcfd1 141 InterruptIn _rightEncoderClk;
abotics 0:72b953cdcfd1 142
abotics 0:72b953cdcfd1 143 PwmOut _rightMotorPWM;
abotics 0:72b953cdcfd1 144 PwmOut _leftMotorPWM;
abotics 0:72b953cdcfd1 145 PwmOut _gripperPWM;
abotics 0:72b953cdcfd1 146
abotics 0:72b953cdcfd1 147 Ticker _motorControl;
abotics 0:72b953cdcfd1 148
abotics 0:72b953cdcfd1 149 volatile char rxBuff[16];
abotics 0:72b953cdcfd1 150 volatile char rxBuffIndx;
abotics 0:72b953cdcfd1 151 volatile char tmpRxBuff[16];
abotics 0:72b953cdcfd1 152 volatile bool uartDataRdy;
abotics 0:72b953cdcfd1 153
abotics 0:72b953cdcfd1 154 volatile int motorUpdateTickCount;
abotics 0:72b953cdcfd1 155 volatile int motionLoopCount;
abotics 0:72b953cdcfd1 156
abotics 0:72b953cdcfd1 157 volatile int leftEncoderCount;
abotics 0:72b953cdcfd1 158 volatile int rightEncoderCount;
abotics 0:72b953cdcfd1 159 volatile bool leftEncoderDir;
abotics 0:72b953cdcfd1 160 volatile bool rightEncoderDir;
abotics 0:72b953cdcfd1 161 volatile int leftMotorOffset;
abotics 0:72b953cdcfd1 162 volatile int rightMotorOffset;
abotics 0:72b953cdcfd1 163
abotics 0:72b953cdcfd1 164 volatile unsigned short analogIndex;
abotics 0:72b953cdcfd1 165 volatile float adSensors[6];
abotics 0:72b953cdcfd1 166 volatile int analogUpdateCount;
abotics 0:72b953cdcfd1 167
abotics 0:72b953cdcfd1 168 volatile char encoderArrayIndex_L, encoderArrayIndex_R;
abotics 0:72b953cdcfd1 169 volatile int encoderPeriodArray_L[4], encoderPeriodArray_R[4];
abotics 0:72b953cdcfd1 170 volatile int encoderPeriod_L, encoderPeriod_R, encoderPeriodSum_L, encoderPeriodSum_R;
abotics 0:72b953cdcfd1 171 volatile int prevT3Count_L, prevT3Count_R;
abotics 0:72b953cdcfd1 172 volatile bool encoderUpdated_L, encoderUpdated_R;
abotics 0:72b953cdcfd1 173 volatile int encoderSpeed_L, encoderSpeed_R;
abotics 0:72b953cdcfd1 174
abotics 0:72b953cdcfd1 175 // Buzzer Variables //
abotics 0:72b953cdcfd1 176 volatile int buzzerPeriod;
abotics 0:72b953cdcfd1 177 volatile int buzzerDuty;
abotics 0:72b953cdcfd1 178 volatile int buzzerTick;
abotics 0:72b953cdcfd1 179 volatile bool enableBuzzer;
abotics 0:72b953cdcfd1 180
abotics 0:72b953cdcfd1 181 void getUartData(void);
abotics 0:72b953cdcfd1 182 void motorControlISR(void);
abotics 0:72b953cdcfd1 183 void leftEncoderTick(void);
abotics 0:72b953cdcfd1 184 void rightEncoderTick(void);
abotics 0:72b953cdcfd1 185
abotics 0:72b953cdcfd1 186 void ParseMotorCommand(void);
abotics 0:72b953cdcfd1 187 void ParseBuzzerCommand(void);
abotics 0:72b953cdcfd1 188 void ParseGripperCommand(void);
abotics 0:72b953cdcfd1 189 void CalculateWheelSpeed(void);
abotics 0:72b953cdcfd1 190 void Initialize(void);
abotics 0:72b953cdcfd1 191
abotics 0:72b953cdcfd1 192 };
abotics 0:72b953cdcfd1 193
abotics 0:72b953cdcfd1 194 #endif