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:
Sun Oct 12 19:49:16 2014 +0000
Revision:
0:72b953cdcfd1
Child:
1:8edccfd4646a
First version of Apeiros Robot Class Library.

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 0:72b953cdcfd1 13 * if (apeiros.IsSerialDataAvailable()) apeiros.ParseUartData();
abotics 0:72b953cdcfd1 14 * }
abotics 0:72b953cdcfd1 15 * @endcode
abotics 0:72b953cdcfd1 16 */
abotics 0:72b953cdcfd1 17
abotics 0:72b953cdcfd1 18 #ifndef MBED_APEIROS_H
abotics 0:72b953cdcfd1 19 #define MBED_APEIROS_H
abotics 0:72b953cdcfd1 20
abotics 0:72b953cdcfd1 21 #include "mbed.h"
abotics 0:72b953cdcfd1 22
abotics 0:72b953cdcfd1 23 #define Dwh 2.621 // wheel diameter
abotics 0:72b953cdcfd1 24 #define PI 3.14159 // PI
abotics 0:72b953cdcfd1 25 #define Cwh (Dwh * PI) // wheel circumference
abotics 0:72b953cdcfd1 26 #define TSCALE 10 // convert to 10ths of an inch
abotics 0:72b953cdcfd1 27 #define INVTICK 4883 // this is 1 / 204.8 us, to avoid using floating point math
abotics 0:72b953cdcfd1 28 #define NCLKS 128 // number of encoder clocks per wheel rotation
abotics 0:72b953cdcfd1 29 #define Ktps ((Cwh * TSCALE * INVTICK) / NCLKS)
abotics 0:72b953cdcfd1 30 #define MaxMotorSpeed 255
abotics 0:72b953cdcfd1 31
abotics 0:72b953cdcfd1 32 #define MIN_GRIPPER_PULSE 1000
abotics 0:72b953cdcfd1 33 #define MAX_GRIPPER_PULSE 2300
abotics 0:72b953cdcfd1 34
abotics 0:72b953cdcfd1 35 #define MAX_BUZZER_PWM 100
abotics 0:72b953cdcfd1 36
abotics 0:72b953cdcfd1 37 class Apeiros : public Serial{
abotics 0:72b953cdcfd1 38
abotics 0:72b953cdcfd1 39 public:
abotics 0:72b953cdcfd1 40
abotics 0:72b953cdcfd1 41 /** Create Apeiros instance
abotics 0:72b953cdcfd1 42 *
abotics 0:72b953cdcfd1 43 * @param tx specified as PinName of UART Tx pin. Use SERIAL_TX as default.
abotics 0:72b953cdcfd1 44 * @param rx specified as PinName of UART Rx pin. Use SERIAL_RX as deafult.
abotics 0:72b953cdcfd1 45 * @param leftMotorPwmOffset specified as integer value from 0-150. Use 120 as default.
abotics 0:72b953cdcfd1 46 * @param rightMotorPwmOffset specified as integer value from 0-150. Use 120 as default.
abotics 0:72b953cdcfd1 47 */
abotics 0:72b953cdcfd1 48 Apeiros(PinName tx, PinName rx, int leftMotorPwmOffset, int rightMotorPwmOffset);
abotics 0:72b953cdcfd1 49
abotics 0:72b953cdcfd1 50 /** Begin using Apeiros class. Must be called before using class functions.
abotics 0:72b953cdcfd1 51 *
abotics 0:72b953cdcfd1 52 */
abotics 0:72b953cdcfd1 53 void Begin(void);
abotics 0:72b953cdcfd1 54
abotics 0:72b953cdcfd1 55 /** Is Serial data available to be parsed.
abotics 0:72b953cdcfd1 56 *
abotics 0:72b953cdcfd1 57 * @returns
abotics 0:72b953cdcfd1 58 * 1 if serial data is available,
abotics 0:72b953cdcfd1 59 * 0 if no serial data has been received
abotics 0:72b953cdcfd1 60 */
abotics 0:72b953cdcfd1 61 bool IsSerialDataAvailable(void);
abotics 0:72b953cdcfd1 62
abotics 0:72b953cdcfd1 63 /** Parse available serial UART data.
abotics 0:72b953cdcfd1 64 *
abotics 0:72b953cdcfd1 65 */
abotics 0:72b953cdcfd1 66 void ParseUartData(void);
abotics 0:72b953cdcfd1 67
abotics 0:72b953cdcfd1 68 /** Set piezo buzzer tone.
abotics 0:72b953cdcfd1 69 *
abotics 0:72b953cdcfd1 70 * @param buzzerTone specified as an integer value(int) from 0-100.
abotics 0:72b953cdcfd1 71 */
abotics 0:72b953cdcfd1 72 void SetBuzzerTone(int buzzerTone);
abotics 0:72b953cdcfd1 73
abotics 0:72b953cdcfd1 74 /** Set left and roght motor speeds.
abotics 0:72b953cdcfd1 75 *
abotics 0:72b953cdcfd1 76 * @param leftMotorSpeed specified as an integer value(int) from 0-150, rightMotorSpeed specified as an integer value(int) from 0-150.
abotics 0:72b953cdcfd1 77 */
abotics 0:72b953cdcfd1 78 void SetMotorSpeed(int leftMotorSpeed, int rightMotorSpeed);
abotics 0:72b953cdcfd1 79
abotics 0:72b953cdcfd1 80 /** Set servo gripper position.
abotics 0:72b953cdcfd1 81 *
abotics 0:72b953cdcfd1 82 * @param pulseWidth_us specified as an integer value(int) from MIN_GRIPPER_PULSE(1000) to MAX_GRIPPER_PULSE(2300).
abotics 0:72b953cdcfd1 83 */
abotics 0:72b953cdcfd1 84 void SetGripperPosition(int pulseWidth_us);
abotics 0:72b953cdcfd1 85
abotics 0:72b953cdcfd1 86 DigitalIn leftFrontIR;
abotics 0:72b953cdcfd1 87 DigitalIn centerFrontIR;
abotics 0:72b953cdcfd1 88 DigitalIn rightFrontIR;
abotics 0:72b953cdcfd1 89
abotics 0:72b953cdcfd1 90 AnalogIn ad_0;
abotics 0:72b953cdcfd1 91 AnalogIn ad_1;
abotics 0:72b953cdcfd1 92 AnalogIn ad_2;
abotics 0:72b953cdcfd1 93 AnalogIn ad_3;
abotics 0:72b953cdcfd1 94 AnalogIn ad_4;
abotics 0:72b953cdcfd1 95 AnalogIn ad_5;
abotics 0:72b953cdcfd1 96
abotics 0:72b953cdcfd1 97 private:
abotics 0:72b953cdcfd1 98
abotics 0:72b953cdcfd1 99 DigitalOut _buzzerOut;
abotics 0:72b953cdcfd1 100
abotics 0:72b953cdcfd1 101 DigitalOut _SN_3A;
abotics 0:72b953cdcfd1 102 DigitalOut _SN_4A;
abotics 0:72b953cdcfd1 103
abotics 0:72b953cdcfd1 104 DigitalOut _SN_2A;
abotics 0:72b953cdcfd1 105 DigitalOut _SN_1A;
abotics 0:72b953cdcfd1 106
abotics 0:72b953cdcfd1 107 DigitalIn _leftEncoderDirPin;
abotics 0:72b953cdcfd1 108 DigitalIn _rightEncoderDirPin;
abotics 0:72b953cdcfd1 109
abotics 0:72b953cdcfd1 110 InterruptIn _leftEncoderClk;
abotics 0:72b953cdcfd1 111 InterruptIn _rightEncoderClk;
abotics 0:72b953cdcfd1 112
abotics 0:72b953cdcfd1 113 PwmOut _rightMotorPWM;
abotics 0:72b953cdcfd1 114 PwmOut _leftMotorPWM;
abotics 0:72b953cdcfd1 115 PwmOut _gripperPWM;
abotics 0:72b953cdcfd1 116
abotics 0:72b953cdcfd1 117 Ticker _motorControl;
abotics 0:72b953cdcfd1 118
abotics 0:72b953cdcfd1 119 volatile char rxBuff[16];
abotics 0:72b953cdcfd1 120 volatile char rxBuffIndx;
abotics 0:72b953cdcfd1 121 volatile char tmpRxBuff[16];
abotics 0:72b953cdcfd1 122 volatile bool uartDataRdy;
abotics 0:72b953cdcfd1 123
abotics 0:72b953cdcfd1 124 volatile int motorUpdateTickCount;
abotics 0:72b953cdcfd1 125 volatile int motionLoopCount;
abotics 0:72b953cdcfd1 126
abotics 0:72b953cdcfd1 127 volatile int leftEncoderCount;
abotics 0:72b953cdcfd1 128 volatile int rightEncoderCount;
abotics 0:72b953cdcfd1 129 volatile bool leftEncoderDir;
abotics 0:72b953cdcfd1 130 volatile bool rightEncoderDir;
abotics 0:72b953cdcfd1 131 volatile int leftMotorOffset;
abotics 0:72b953cdcfd1 132 volatile int rightMotorOffset;
abotics 0:72b953cdcfd1 133
abotics 0:72b953cdcfd1 134 volatile unsigned short analogIndex;
abotics 0:72b953cdcfd1 135 volatile float adSensors[6];
abotics 0:72b953cdcfd1 136 volatile int analogUpdateCount;
abotics 0:72b953cdcfd1 137
abotics 0:72b953cdcfd1 138 volatile char encoderArrayIndex_L, encoderArrayIndex_R;
abotics 0:72b953cdcfd1 139 volatile int encoderPeriodArray_L[4], encoderPeriodArray_R[4];
abotics 0:72b953cdcfd1 140 volatile int encoderPeriod_L, encoderPeriod_R, encoderPeriodSum_L, encoderPeriodSum_R;
abotics 0:72b953cdcfd1 141 volatile int prevT3Count_L, prevT3Count_R;
abotics 0:72b953cdcfd1 142 volatile bool encoderUpdated_L, encoderUpdated_R;
abotics 0:72b953cdcfd1 143 volatile int encoderSpeed_L, encoderSpeed_R;
abotics 0:72b953cdcfd1 144
abotics 0:72b953cdcfd1 145 // Buzzer Variables //
abotics 0:72b953cdcfd1 146 volatile int buzzerPeriod;
abotics 0:72b953cdcfd1 147 volatile int buzzerDuty;
abotics 0:72b953cdcfd1 148 volatile int buzzerTick;
abotics 0:72b953cdcfd1 149 volatile bool enableBuzzer;
abotics 0:72b953cdcfd1 150
abotics 0:72b953cdcfd1 151 void getUartData(void);
abotics 0:72b953cdcfd1 152 void motorControlISR(void);
abotics 0:72b953cdcfd1 153 void leftEncoderTick(void);
abotics 0:72b953cdcfd1 154 void rightEncoderTick(void);
abotics 0:72b953cdcfd1 155
abotics 0:72b953cdcfd1 156 void ParseMotorCommand(void);
abotics 0:72b953cdcfd1 157 void ParseBuzzerCommand(void);
abotics 0:72b953cdcfd1 158 void ParseGripperCommand(void);
abotics 0:72b953cdcfd1 159 void CalculateWheelSpeed(void);
abotics 0:72b953cdcfd1 160 void Initialize(void);
abotics 0:72b953cdcfd1 161
abotics 0:72b953cdcfd1 162 };
abotics 0:72b953cdcfd1 163
abotics 0:72b953cdcfd1 164 #endif