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 #include "Apeiros.h"
abotics 0:72b953cdcfd1 2 #include "mbed.h"
abotics 0:72b953cdcfd1 3
abotics 0:72b953cdcfd1 4 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 5 // Constructor for Apeiros Class.
abotics 0:72b953cdcfd1 6 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 7 Apeiros::Apeiros(PinName tx, PinName rx, int leftMotorPwmOffset, int rightMotorPwmOffset) : Serial(tx, rx), leftFrontIR(PC_15),centerFrontIR(PC_14),rightFrontIR(PC_13),
abotics 0:72b953cdcfd1 8 ad_0(PA_0), ad_1(PA_1),ad_2(PA_4),ad_3(PB_0),ad_4(PC_1),ad_5(PC_0),_buzzerOut(PA_5),_SN_3A(PB_5),_SN_4A(PC_7),_SN_2A(PB_3),_SN_1A(PA_8),
abotics 0:72b953cdcfd1 9 _leftEncoderDirPin(PA_6),_rightEncoderDirPin(PA_7),_leftEncoderClk(PH_1),_rightEncoderClk(PH_0),_rightMotorPWM(PB_10),_leftMotorPWM(PB_4),_gripperPWM(PB_6)
abotics 0:72b953cdcfd1 10 {
abotics 0:72b953cdcfd1 11 // Set direction of PWM dir pins to be low so robot is halted. //
abotics 0:72b953cdcfd1 12 _SN_3A = 0;
abotics 0:72b953cdcfd1 13 _SN_4A = 0;
abotics 0:72b953cdcfd1 14 _SN_2A = 0;
abotics 0:72b953cdcfd1 15 _SN_1A = 0;
abotics 0:72b953cdcfd1 16
abotics 0:72b953cdcfd1 17 // Configure Left & Right Motor PWMs. //
abotics 0:72b953cdcfd1 18 _rightMotorPWM.period_us(255);
abotics 0:72b953cdcfd1 19 _rightMotorPWM.pulsewidth_us(0);
abotics 0:72b953cdcfd1 20 _leftMotorPWM.period_us(255);
abotics 0:72b953cdcfd1 21 _leftMotorPWM.pulsewidth_us(0);
abotics 0:72b953cdcfd1 22
abotics 0:72b953cdcfd1 23 // Configure Gripper PWM. //
abotics 0:72b953cdcfd1 24 _gripperPWM.period_ms(20);
abotics 0:72b953cdcfd1 25 SetGripperPosition(2300);
abotics 0:72b953cdcfd1 26
abotics 0:72b953cdcfd1 27 // Configure optional Wheel Encoder Inputs & ISRs. //
abotics 0:72b953cdcfd1 28 _leftEncoderDirPin.mode(PullUp);
abotics 0:72b953cdcfd1 29 _rightEncoderDirPin.mode(PullUp);
abotics 0:72b953cdcfd1 30 _leftEncoderClk.mode(PullDown);
abotics 0:72b953cdcfd1 31 _rightEncoderClk.mode(PullUp);
abotics 0:72b953cdcfd1 32 _leftEncoderClk.rise(this, &Apeiros::leftEncoderTick);
abotics 0:72b953cdcfd1 33 _rightEncoderClk.fall(this, &Apeiros::rightEncoderTick);
abotics 0:72b953cdcfd1 34
abotics 0:72b953cdcfd1 35 // Configure Serial ISR & baud rate. //
abotics 0:72b953cdcfd1 36 Serial::attach(this, &Apeiros::getUartData,Serial::RxIrq);
abotics 0:72b953cdcfd1 37 baud(115200);
abotics 0:72b953cdcfd1 38
abotics 0:72b953cdcfd1 39 rxBuffIndx = 0;
abotics 0:72b953cdcfd1 40 uartDataRdy = false;
abotics 0:72b953cdcfd1 41 motorUpdateTickCount = 0;
abotics 0:72b953cdcfd1 42 motionLoopCount = 0;
abotics 0:72b953cdcfd1 43
abotics 0:72b953cdcfd1 44 leftEncoderCount = 0;
abotics 0:72b953cdcfd1 45 rightEncoderCount = 0;
abotics 0:72b953cdcfd1 46 leftEncoderDir = 1;
abotics 0:72b953cdcfd1 47 rightEncoderDir = 1;
abotics 0:72b953cdcfd1 48 leftMotorOffset = leftMotorPwmOffset;
abotics 0:72b953cdcfd1 49 rightMotorOffset = rightMotorPwmOffset;
abotics 0:72b953cdcfd1 50
abotics 0:72b953cdcfd1 51 analogIndex = 0;
abotics 0:72b953cdcfd1 52 buzzerPeriod = 2;
abotics 0:72b953cdcfd1 53 buzzerDuty = buzzerPeriod/2;
abotics 0:72b953cdcfd1 54 buzzerTick = 0;
abotics 0:72b953cdcfd1 55 enableBuzzer = false;
abotics 0:72b953cdcfd1 56
abotics 0:72b953cdcfd1 57 analogUpdateCount = 0;
abotics 0:72b953cdcfd1 58 }
abotics 0:72b953cdcfd1 59
abotics 0:72b953cdcfd1 60 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 61 // Function to Begin using Apeiros class.
abotics 0:72b953cdcfd1 62 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 63 void Apeiros::Begin()
abotics 0:72b953cdcfd1 64 {
abotics 0:72b953cdcfd1 65 // Configure motor control ISR & set to interrupt every 205us. //
abotics 0:72b953cdcfd1 66 _motorControl.attach_us(this, &Apeiros::motorControlISR, 205);
abotics 0:72b953cdcfd1 67
abotics 0:72b953cdcfd1 68 printf("\r\n");
abotics 0:72b953cdcfd1 69 printf("Hello, my name is Apeiros!\r\n");
abotics 0:72b953cdcfd1 70
abotics 0:72b953cdcfd1 71 // Play initialize buzzer tones. //
abotics 0:72b953cdcfd1 72 SetBuzzerTone(1);
abotics 0:72b953cdcfd1 73 wait_ms(250);
abotics 0:72b953cdcfd1 74 SetBuzzerTone(10);
abotics 0:72b953cdcfd1 75 wait_ms(250);
abotics 0:72b953cdcfd1 76 SetBuzzerTone(1);
abotics 0:72b953cdcfd1 77 wait_ms(250);
abotics 0:72b953cdcfd1 78 SetBuzzerTone(0);
abotics 0:72b953cdcfd1 79 }
abotics 0:72b953cdcfd1 80
abotics 0:72b953cdcfd1 81 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 82 // Serial Interrupt Service Routine (ISR) for data received via UART.
abotics 0:72b953cdcfd1 83 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 84 void Apeiros::getUartData()
abotics 0:72b953cdcfd1 85 {
abotics 0:72b953cdcfd1 86 if (!uartDataRdy)
abotics 0:72b953cdcfd1 87 {
abotics 0:72b953cdcfd1 88 rxBuff[rxBuffIndx] = getc();
abotics 0:72b953cdcfd1 89
abotics 0:72b953cdcfd1 90 if (rxBuff[rxBuffIndx] == 13)
abotics 0:72b953cdcfd1 91 {
abotics 0:72b953cdcfd1 92 for (rxBuffIndx = 0; rxBuffIndx < 16; rxBuffIndx ++) tmpRxBuff[rxBuffIndx] = rxBuff[rxBuffIndx];
abotics 0:72b953cdcfd1 93 rxBuffIndx = 0;
abotics 0:72b953cdcfd1 94 uartDataRdy = true;
abotics 0:72b953cdcfd1 95 }
abotics 0:72b953cdcfd1 96 else
abotics 0:72b953cdcfd1 97 {
abotics 0:72b953cdcfd1 98 rxBuffIndx++;
abotics 0:72b953cdcfd1 99 if (rxBuffIndx > 15)
abotics 0:72b953cdcfd1 100 {
abotics 0:72b953cdcfd1 101 rxBuffIndx = 0;
abotics 0:72b953cdcfd1 102 for (int indx=0;indx<16;indx++) rxBuff[indx] = 0;
abotics 0:72b953cdcfd1 103 }
abotics 0:72b953cdcfd1 104 }
abotics 0:72b953cdcfd1 105 }
abotics 0:72b953cdcfd1 106 }
abotics 0:72b953cdcfd1 107
abotics 0:72b953cdcfd1 108 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 109 // Motor Control Interrupt Service Routine (ISR).
abotics 0:72b953cdcfd1 110 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 111 void Apeiros::motorControlISR()
abotics 0:72b953cdcfd1 112 {
abotics 0:72b953cdcfd1 113 motorUpdateTickCount++;
abotics 0:72b953cdcfd1 114 motionLoopCount++;
abotics 0:72b953cdcfd1 115 analogUpdateCount++;
abotics 0:72b953cdcfd1 116
abotics 0:72b953cdcfd1 117 if (motionLoopCount > 250)
abotics 0:72b953cdcfd1 118 {
abotics 0:72b953cdcfd1 119 CalculateWheelSpeed();
abotics 0:72b953cdcfd1 120 motionLoopCount = 0;
abotics 0:72b953cdcfd1 121 }
abotics 0:72b953cdcfd1 122
abotics 0:72b953cdcfd1 123 if (analogUpdateCount > 50)
abotics 0:72b953cdcfd1 124 {
abotics 0:72b953cdcfd1 125 // Update analog to digital (A/D) conversions. //
abotics 0:72b953cdcfd1 126 switch (analogIndex)
abotics 0:72b953cdcfd1 127 {
abotics 0:72b953cdcfd1 128 case 0:
abotics 0:72b953cdcfd1 129 adSensors[0] = ad_0.read();
abotics 0:72b953cdcfd1 130 break;
abotics 0:72b953cdcfd1 131
abotics 0:72b953cdcfd1 132 case 1:
abotics 0:72b953cdcfd1 133 adSensors[1] = ad_1.read();
abotics 0:72b953cdcfd1 134 break;
abotics 0:72b953cdcfd1 135
abotics 0:72b953cdcfd1 136 case 2:
abotics 0:72b953cdcfd1 137 adSensors[2] = ad_2.read();
abotics 0:72b953cdcfd1 138 break;
abotics 0:72b953cdcfd1 139
abotics 0:72b953cdcfd1 140 case 3:
abotics 0:72b953cdcfd1 141 adSensors[3] = ad_3.read();
abotics 0:72b953cdcfd1 142 break;
abotics 0:72b953cdcfd1 143
abotics 0:72b953cdcfd1 144 case 4:
abotics 0:72b953cdcfd1 145 adSensors[4] = ad_4.read();
abotics 0:72b953cdcfd1 146 break;
abotics 0:72b953cdcfd1 147
abotics 0:72b953cdcfd1 148 case 5:
abotics 0:72b953cdcfd1 149 adSensors[5] = ad_5.read();
abotics 0:72b953cdcfd1 150 break;
abotics 0:72b953cdcfd1 151 }
abotics 0:72b953cdcfd1 152 analogIndex++;
abotics 0:72b953cdcfd1 153 if (analogIndex > 5) analogIndex = 0;
abotics 0:72b953cdcfd1 154
abotics 0:72b953cdcfd1 155 analogUpdateCount = 0;
abotics 0:72b953cdcfd1 156 }
abotics 0:72b953cdcfd1 157
abotics 0:72b953cdcfd1 158 // Update piezo buzzer tone as needed. //
abotics 0:72b953cdcfd1 159 if (enableBuzzer)
abotics 0:72b953cdcfd1 160 {
abotics 0:72b953cdcfd1 161 buzzerTick++;
abotics 0:72b953cdcfd1 162 if (buzzerTick > buzzerPeriod)
abotics 0:72b953cdcfd1 163 {
abotics 0:72b953cdcfd1 164 buzzerTick = 0;
abotics 0:72b953cdcfd1 165 _buzzerOut = 1;
abotics 0:72b953cdcfd1 166 }
abotics 0:72b953cdcfd1 167 else if (buzzerTick > buzzerDuty)
abotics 0:72b953cdcfd1 168 {
abotics 0:72b953cdcfd1 169 _buzzerOut = 0;
abotics 0:72b953cdcfd1 170 }
abotics 0:72b953cdcfd1 171 }
abotics 0:72b953cdcfd1 172 }
abotics 0:72b953cdcfd1 173
abotics 0:72b953cdcfd1 174 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 175 // Interrupt Service Routine (ISR) for Left Wheel Encoder Transitions.
abotics 0:72b953cdcfd1 176 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 177 void Apeiros::leftEncoderTick()
abotics 0:72b953cdcfd1 178 {
abotics 0:72b953cdcfd1 179 leftEncoderDir = _leftEncoderDirPin.read();
abotics 0:72b953cdcfd1 180 if (!leftEncoderDir) leftEncoderCount++;
abotics 0:72b953cdcfd1 181 else leftEncoderCount--;
abotics 0:72b953cdcfd1 182
abotics 0:72b953cdcfd1 183 // Remove the oldest value from the sum.
abotics 0:72b953cdcfd1 184 encoderPeriodSum_L -= encoderPeriodArray_L[encoderArrayIndex_L];
abotics 0:72b953cdcfd1 185
abotics 0:72b953cdcfd1 186 // Store the newest value, and add it to the sum.
abotics 0:72b953cdcfd1 187 encoderPeriodSum_L += encoderPeriodArray_L[encoderArrayIndex_L] = motorUpdateTickCount - prevT3Count_L;
abotics 0:72b953cdcfd1 188
abotics 0:72b953cdcfd1 189 // Calculate a new average period.
abotics 0:72b953cdcfd1 190 encoderPeriod_L = encoderPeriodSum_L/4;
abotics 0:72b953cdcfd1 191
abotics 0:72b953cdcfd1 192 // Move to the next array position.
abotics 0:72b953cdcfd1 193 encoderArrayIndex_L++;
abotics 0:72b953cdcfd1 194 if (encoderArrayIndex_L > 3) encoderArrayIndex_L = 0;
abotics 0:72b953cdcfd1 195
abotics 0:72b953cdcfd1 196 // Store the current timer3TickCount for next time.
abotics 0:72b953cdcfd1 197 prevT3Count_L = motorUpdateTickCount;
abotics 0:72b953cdcfd1 198
abotics 0:72b953cdcfd1 199 // Set encoder as updated.
abotics 0:72b953cdcfd1 200 encoderUpdated_L = true;
abotics 0:72b953cdcfd1 201 }
abotics 0:72b953cdcfd1 202
abotics 0:72b953cdcfd1 203 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 204 // Interrupt Service Routine (ISR) for Right Wheel Encoder Transitions.
abotics 0:72b953cdcfd1 205 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 206 void Apeiros::rightEncoderTick()
abotics 0:72b953cdcfd1 207 {
abotics 0:72b953cdcfd1 208 rightEncoderDir = _rightEncoderDirPin.read();
abotics 0:72b953cdcfd1 209 if (!rightEncoderDir) rightEncoderCount--;
abotics 0:72b953cdcfd1 210 else rightEncoderCount++;
abotics 0:72b953cdcfd1 211
abotics 0:72b953cdcfd1 212 // Remove the oldest value from the sum.
abotics 0:72b953cdcfd1 213 encoderPeriodSum_R -= encoderPeriodArray_R[encoderArrayIndex_R];
abotics 0:72b953cdcfd1 214
abotics 0:72b953cdcfd1 215 // Store the newest value, and add it to the sum.
abotics 0:72b953cdcfd1 216 encoderPeriodSum_R += encoderPeriodArray_R[encoderArrayIndex_R] = motorUpdateTickCount - prevT3Count_R;
abotics 0:72b953cdcfd1 217
abotics 0:72b953cdcfd1 218 // Calculate a new average period.
abotics 0:72b953cdcfd1 219 encoderPeriod_R = encoderPeriodSum_R/4;
abotics 0:72b953cdcfd1 220
abotics 0:72b953cdcfd1 221 // Move to the next array position.
abotics 0:72b953cdcfd1 222 encoderArrayIndex_R++;
abotics 0:72b953cdcfd1 223 if (encoderArrayIndex_R > 3) encoderArrayIndex_R = 0;
abotics 0:72b953cdcfd1 224
abotics 0:72b953cdcfd1 225 // Store the current timer3TickCount for next time.
abotics 0:72b953cdcfd1 226 prevT3Count_R = motorUpdateTickCount;
abotics 0:72b953cdcfd1 227
abotics 0:72b953cdcfd1 228 // Set encoder as updated.
abotics 0:72b953cdcfd1 229 encoderUpdated_R = true;
abotics 0:72b953cdcfd1 230 }
abotics 0:72b953cdcfd1 231
abotics 0:72b953cdcfd1 232 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 233 // Function to return status of UART data.
abotics 0:72b953cdcfd1 234 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 235 bool Apeiros::IsSerialDataAvailable()
abotics 0:72b953cdcfd1 236 {
abotics 0:72b953cdcfd1 237 if (uartDataRdy) return true;
abotics 0:72b953cdcfd1 238 else return false;
abotics 0:72b953cdcfd1 239 }
abotics 0:72b953cdcfd1 240
abotics 0:72b953cdcfd1 241 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 242 // Function to parse UART data from Rx buffer for SDIO1 (PA9 & PA10).
abotics 0:72b953cdcfd1 243 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 244 void Apeiros::ParseUartData()
abotics 0:72b953cdcfd1 245 {
abotics 0:72b953cdcfd1 246 switch (tmpRxBuff[0]){
abotics 0:72b953cdcfd1 247
abotics 0:72b953cdcfd1 248 case 'A':
abotics 0:72b953cdcfd1 249 printf("Sensors = (%0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f)\r\n", adSensors[0], adSensors[1], adSensors[2], adSensors[3], adSensors[4], adSensors[5]);
abotics 0:72b953cdcfd1 250 break;
abotics 0:72b953cdcfd1 251
abotics 0:72b953cdcfd1 252 case 'B':
abotics 0:72b953cdcfd1 253 ParseBuzzerCommand();
abotics 0:72b953cdcfd1 254 printf("b\r\n");
abotics 0:72b953cdcfd1 255 break;
abotics 0:72b953cdcfd1 256
abotics 0:72b953cdcfd1 257 case 'D':
abotics 0:72b953cdcfd1 258 printf("Sensors = (%d, %d, %d)\r\n", leftFrontIR.read(), centerFrontIR.read(), rightFrontIR.read());
abotics 0:72b953cdcfd1 259 break;
abotics 0:72b953cdcfd1 260
abotics 0:72b953cdcfd1 261 case 'E':
abotics 0:72b953cdcfd1 262 printf("Encoder Ticks = %d, %d\r\n",leftEncoderCount, rightEncoderCount);
abotics 0:72b953cdcfd1 263 break;
abotics 0:72b953cdcfd1 264
abotics 0:72b953cdcfd1 265 case 'G':
abotics 0:72b953cdcfd1 266 ParseGripperCommand();
abotics 0:72b953cdcfd1 267 printf("g\r\n");
abotics 0:72b953cdcfd1 268 break;
abotics 0:72b953cdcfd1 269
abotics 0:72b953cdcfd1 270 case 'H':
abotics 0:72b953cdcfd1 271 SetMotorSpeed(0, 0);
abotics 0:72b953cdcfd1 272 printf("h\r\n");
abotics 0:72b953cdcfd1 273 break;
abotics 0:72b953cdcfd1 274
abotics 0:72b953cdcfd1 275 case 'M':
abotics 0:72b953cdcfd1 276 ParseMotorCommand();
abotics 0:72b953cdcfd1 277 printf("ma\r\n");
abotics 0:72b953cdcfd1 278 break;
abotics 0:72b953cdcfd1 279
abotics 0:72b953cdcfd1 280 case 'V':
abotics 0:72b953cdcfd1 281 printf("Motor Velocity = %d, %d\r\n",encoderSpeed_L, encoderSpeed_R);
abotics 0:72b953cdcfd1 282 break;
abotics 0:72b953cdcfd1 283
abotics 0:72b953cdcfd1 284 case 'Y':
abotics 0:72b953cdcfd1 285 _gripperPWM.pulsewidth_us(2000);
abotics 0:72b953cdcfd1 286 printf("ya\r\n");
abotics 0:72b953cdcfd1 287 break;
abotics 0:72b953cdcfd1 288
abotics 0:72b953cdcfd1 289 case 'Z':
abotics 0:72b953cdcfd1 290 _gripperPWM.pulsewidth_us(1000);
abotics 0:72b953cdcfd1 291 printf("za\r\n");
abotics 0:72b953cdcfd1 292 break;
abotics 0:72b953cdcfd1 293
abotics 0:72b953cdcfd1 294 default:
abotics 0:72b953cdcfd1 295 printf("Cmd unrecognized!\r\n");
abotics 0:72b953cdcfd1 296 break;
abotics 0:72b953cdcfd1 297 }
abotics 0:72b953cdcfd1 298
abotics 0:72b953cdcfd1 299 for (int buffIndx = 0; buffIndx < 16; buffIndx++) tmpRxBuff[buffIndx] = 0;
abotics 0:72b953cdcfd1 300
abotics 0:72b953cdcfd1 301 uartDataRdy = false;
abotics 0:72b953cdcfd1 302 }
abotics 0:72b953cdcfd1 303
abotics 0:72b953cdcfd1 304 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 305 // Function to set tone of piezo buzzer.
abotics 0:72b953cdcfd1 306 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 307 void Apeiros::SetBuzzerTone(int buzzerTone)
abotics 0:72b953cdcfd1 308 {
abotics 0:72b953cdcfd1 309 if (buzzerTone > 0)
abotics 0:72b953cdcfd1 310 {
abotics 0:72b953cdcfd1 311 if (buzzerTone > MAX_BUZZER_PWM) buzzerTone = MAX_BUZZER_PWM;
abotics 0:72b953cdcfd1 312 buzzerTick = 1;
abotics 0:72b953cdcfd1 313 buzzerPeriod = buzzerTone;
abotics 0:72b953cdcfd1 314 buzzerDuty = buzzerPeriod/2;
abotics 0:72b953cdcfd1 315 enableBuzzer = true;
abotics 0:72b953cdcfd1 316 }
abotics 0:72b953cdcfd1 317 else
abotics 0:72b953cdcfd1 318 {
abotics 0:72b953cdcfd1 319 enableBuzzer = false;
abotics 0:72b953cdcfd1 320 _buzzerOut = 0;
abotics 0:72b953cdcfd1 321 }
abotics 0:72b953cdcfd1 322 }
abotics 0:72b953cdcfd1 323
abotics 0:72b953cdcfd1 324 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 325 // Calculate Wheel Speeds
abotics 0:72b953cdcfd1 326 //
abotics 0:72b953cdcfd1 327 // Speed in 0.1"/sec = (Cwh / NCLKS) * TSCALE / (TICK * PER) = Ktps / PER
abotics 0:72b953cdcfd1 328 // where Cwh = 8.12" wheel circumference, NCLKS = 128 (32 stripe disk),
abotics 0:72b953cdcfd1 329 // TSCALE = 10 (to convert inches to 0.1"), TICK = 205us per timer2 tick,
abotics 0:72b953cdcfd1 330 // and PER is the measured period in timer 2 ticks
abotics 0:72b953cdcfd1 331 // Ktps = 3098
abotics 0:72b953cdcfd1 332 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 333 void Apeiros::CalculateWheelSpeed()
abotics 0:72b953cdcfd1 334 {
abotics 0:72b953cdcfd1 335 // If the wheel is spinning so slow that we don't have current reading.
abotics 0:72b953cdcfd1 336 if (!encoderUpdated_R) encoderPeriod_R = motorUpdateTickCount - prevT3Count_R; // Calculate period since last update.
abotics 0:72b953cdcfd1 337 else encoderUpdated_R = false; // Otherwise use it.
abotics 0:72b953cdcfd1 338
abotics 0:72b953cdcfd1 339 // Converts from 205us ticks per edge to multiples of 0.1 inches per second.
abotics 0:72b953cdcfd1 340 encoderSpeed_R = (signed short)(Ktps / encoderPeriod_R);
abotics 0:72b953cdcfd1 341
abotics 0:72b953cdcfd1 342 // Check direction of wheel rotation & adjust as needed. */
abotics 0:72b953cdcfd1 343 if (!rightEncoderDir) encoderSpeed_R = -encoderSpeed_R;
abotics 0:72b953cdcfd1 344
abotics 0:72b953cdcfd1 345 if (!encoderUpdated_L) encoderPeriod_L = motorUpdateTickCount - prevT3Count_L;
abotics 0:72b953cdcfd1 346 else encoderUpdated_L = false;
abotics 0:72b953cdcfd1 347
abotics 0:72b953cdcfd1 348 // Converts from 205us ticks per edge to multiples of 0.1 inches per second
abotics 0:72b953cdcfd1 349 encoderSpeed_L = (signed short)(Ktps / encoderPeriod_L);
abotics 0:72b953cdcfd1 350
abotics 0:72b953cdcfd1 351 // Check direction of wheel rotation & adjust as needed. */
abotics 0:72b953cdcfd1 352 if (leftEncoderDir) encoderSpeed_L = -encoderSpeed_L;
abotics 0:72b953cdcfd1 353 }
abotics 0:72b953cdcfd1 354
abotics 0:72b953cdcfd1 355 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 356 // Function to set left & right motor speed and direction.
abotics 0:72b953cdcfd1 357 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 358 void Apeiros::SetMotorSpeed(int leftMotorSpeed, int rightMotorSpeed)
abotics 0:72b953cdcfd1 359 {
abotics 0:72b953cdcfd1 360
abotics 0:72b953cdcfd1 361 int tmpLeftSpeed = 0;
abotics 0:72b953cdcfd1 362 int tmpRightSpeed = 0;
abotics 0:72b953cdcfd1 363
abotics 0:72b953cdcfd1 364 if (rightMotorSpeed < 0)
abotics 0:72b953cdcfd1 365 {
abotics 0:72b953cdcfd1 366 _SN_2A = 0;
abotics 0:72b953cdcfd1 367 _SN_1A = 1;
abotics 0:72b953cdcfd1 368 }
abotics 0:72b953cdcfd1 369 else {
abotics 0:72b953cdcfd1 370 _SN_2A = 1;
abotics 0:72b953cdcfd1 371 _SN_1A = 0; }
abotics 0:72b953cdcfd1 372
abotics 0:72b953cdcfd1 373 if (leftMotorSpeed < 0)
abotics 0:72b953cdcfd1 374 {
abotics 0:72b953cdcfd1 375 _SN_3A = 0;
abotics 0:72b953cdcfd1 376 _SN_4A = 1;
abotics 0:72b953cdcfd1 377 }
abotics 0:72b953cdcfd1 378 else {
abotics 0:72b953cdcfd1 379 _SN_3A = 1;
abotics 0:72b953cdcfd1 380 _SN_4A = 0; }
abotics 0:72b953cdcfd1 381
abotics 0:72b953cdcfd1 382 if (leftMotorSpeed != 0) tmpLeftSpeed = abs(leftMotorSpeed) + leftMotorOffset;
abotics 0:72b953cdcfd1 383 else
abotics 0:72b953cdcfd1 384 {
abotics 0:72b953cdcfd1 385 tmpLeftSpeed = 0;
abotics 0:72b953cdcfd1 386 _SN_3A = _SN_4A = 0;
abotics 0:72b953cdcfd1 387 }
abotics 0:72b953cdcfd1 388
abotics 0:72b953cdcfd1 389 if (rightMotorSpeed != 0) tmpRightSpeed = abs(rightMotorSpeed) + rightMotorOffset;
abotics 0:72b953cdcfd1 390 else
abotics 0:72b953cdcfd1 391 {
abotics 0:72b953cdcfd1 392 tmpRightSpeed = 0;
abotics 0:72b953cdcfd1 393 _SN_1A = _SN_2A = 0;
abotics 0:72b953cdcfd1 394 }
abotics 0:72b953cdcfd1 395
abotics 0:72b953cdcfd1 396 if (tmpLeftSpeed > MaxMotorSpeed) tmpLeftSpeed = MaxMotorSpeed;
abotics 0:72b953cdcfd1 397 if (tmpRightSpeed > MaxMotorSpeed) tmpRightSpeed = MaxMotorSpeed;
abotics 0:72b953cdcfd1 398
abotics 0:72b953cdcfd1 399 _leftMotorPWM.pulsewidth_us(tmpLeftSpeed);
abotics 0:72b953cdcfd1 400 _rightMotorPWM.pulsewidth_us(tmpRightSpeed);
abotics 0:72b953cdcfd1 401
abotics 0:72b953cdcfd1 402 }
abotics 0:72b953cdcfd1 403
abotics 0:72b953cdcfd1 404 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 405 // Function to parse motor commands from UART data.
abotics 0:72b953cdcfd1 406 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 407 void Apeiros::ParseMotorCommand()
abotics 0:72b953cdcfd1 408 {
abotics 0:72b953cdcfd1 409 unsigned int commaPos, endPos, index1, index2;
abotics 0:72b953cdcfd1 410 signed short leftWheel, rightWheel;
abotics 0:72b953cdcfd1 411 char charBuff[5];
abotics 0:72b953cdcfd1 412 bool foundNegative = false;
abotics 0:72b953cdcfd1 413
abotics 0:72b953cdcfd1 414 commaPos = 0;
abotics 0:72b953cdcfd1 415 endPos = 0;
abotics 0:72b953cdcfd1 416
abotics 0:72b953cdcfd1 417 // Find comma position and return char.
abotics 0:72b953cdcfd1 418 for (index1=2;index1<16;index1++)
abotics 0:72b953cdcfd1 419 {
abotics 0:72b953cdcfd1 420 if (tmpRxBuff[index1] == ',') commaPos = index1;
abotics 0:72b953cdcfd1 421 else if (tmpRxBuff[index1] == 13)
abotics 0:72b953cdcfd1 422 {
abotics 0:72b953cdcfd1 423 endPos = index1;
abotics 0:72b953cdcfd1 424 break;
abotics 0:72b953cdcfd1 425 }
abotics 0:72b953cdcfd1 426 }
abotics 0:72b953cdcfd1 427
abotics 0:72b953cdcfd1 428 // Parse out left motor data.
abotics 0:72b953cdcfd1 429 for (index1=0;index1<5;index1++) charBuff[index1] = ' ';
abotics 0:72b953cdcfd1 430 index2 = 0;
abotics 0:72b953cdcfd1 431 for (index1=2;index1<commaPos;index1++)
abotics 0:72b953cdcfd1 432 {
abotics 0:72b953cdcfd1 433 if (tmpRxBuff[index1] != '-') charBuff[index2] = tmpRxBuff[index1];
abotics 0:72b953cdcfd1 434 else foundNegative = true;
abotics 0:72b953cdcfd1 435 index2++;
abotics 0:72b953cdcfd1 436 }
abotics 0:72b953cdcfd1 437 leftWheel = atol(charBuff);
abotics 0:72b953cdcfd1 438 if (foundNegative) leftWheel = -leftWheel;
abotics 0:72b953cdcfd1 439
abotics 0:72b953cdcfd1 440 foundNegative = false;
abotics 0:72b953cdcfd1 441
abotics 0:72b953cdcfd1 442 // Parse out right motor data.
abotics 0:72b953cdcfd1 443 for (index1=0;index1<5;index1++) charBuff[index1] = ' ';
abotics 0:72b953cdcfd1 444 index2 = 0;
abotics 0:72b953cdcfd1 445 for (index1=commaPos+1;index1<endPos;index1++)
abotics 0:72b953cdcfd1 446 {
abotics 0:72b953cdcfd1 447 if (tmpRxBuff[index1] != '-') charBuff[index2] = tmpRxBuff[index1];
abotics 0:72b953cdcfd1 448 else foundNegative = true;
abotics 0:72b953cdcfd1 449 index2++;
abotics 0:72b953cdcfd1 450 }
abotics 0:72b953cdcfd1 451 rightWheel = atol(charBuff);
abotics 0:72b953cdcfd1 452 if (foundNegative) rightWheel = -rightWheel;
abotics 0:72b953cdcfd1 453
abotics 0:72b953cdcfd1 454 SetMotorSpeed(leftWheel, rightWheel);
abotics 0:72b953cdcfd1 455 }
abotics 0:72b953cdcfd1 456
abotics 0:72b953cdcfd1 457 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 458 // Function to parse piezo buzzer tone commands from UART data.
abotics 0:72b953cdcfd1 459 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 460 void Apeiros::ParseBuzzerCommand()
abotics 0:72b953cdcfd1 461 {
abotics 0:72b953cdcfd1 462 unsigned int index1, index2, endPos;
abotics 0:72b953cdcfd1 463 unsigned int buzzerTonePWM;
abotics 0:72b953cdcfd1 464 char charBuff[3];
abotics 0:72b953cdcfd1 465
abotics 0:72b953cdcfd1 466 index1 = index2 = endPos = 0;
abotics 0:72b953cdcfd1 467 buzzerTonePWM = 1;
abotics 0:72b953cdcfd1 468
abotics 0:72b953cdcfd1 469 for (index1=0;index1<3;index1++) charBuff[index1] = ' ';
abotics 0:72b953cdcfd1 470
abotics 0:72b953cdcfd1 471 // Find position of return char.
abotics 0:72b953cdcfd1 472 for (index1=1;index1<16;index1++)
abotics 0:72b953cdcfd1 473 {
abotics 0:72b953cdcfd1 474 if (tmpRxBuff[index1] == 13)
abotics 0:72b953cdcfd1 475 {
abotics 0:72b953cdcfd1 476 endPos = index1;
abotics 0:72b953cdcfd1 477 break;
abotics 0:72b953cdcfd1 478 }
abotics 0:72b953cdcfd1 479 }
abotics 0:72b953cdcfd1 480
abotics 0:72b953cdcfd1 481 index2 = 0;
abotics 0:72b953cdcfd1 482 for (index1=1;index1<endPos;index1++)
abotics 0:72b953cdcfd1 483 {
abotics 0:72b953cdcfd1 484 charBuff[index2] = tmpRxBuff[index1];
abotics 0:72b953cdcfd1 485 index2++;
abotics 0:72b953cdcfd1 486 }
abotics 0:72b953cdcfd1 487
abotics 0:72b953cdcfd1 488 buzzerTonePWM = atol(charBuff);
abotics 0:72b953cdcfd1 489 SetBuzzerTone(buzzerTonePWM);
abotics 0:72b953cdcfd1 490 }
abotics 0:72b953cdcfd1 491
abotics 0:72b953cdcfd1 492 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 493 // Function to parse gripper servo commands from UART data.
abotics 0:72b953cdcfd1 494 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 495 void Apeiros::ParseGripperCommand()
abotics 0:72b953cdcfd1 496 {
abotics 0:72b953cdcfd1 497 unsigned int index1, index2, endPos;
abotics 0:72b953cdcfd1 498 unsigned int gripperPosition;
abotics 0:72b953cdcfd1 499 char charBuff[4];
abotics 0:72b953cdcfd1 500
abotics 0:72b953cdcfd1 501 index1 = index2 = endPos = 0;
abotics 0:72b953cdcfd1 502 gripperPosition = 2000;
abotics 0:72b953cdcfd1 503
abotics 0:72b953cdcfd1 504 for (index1=0;index1<4;index1++) charBuff[index1] = ' ';
abotics 0:72b953cdcfd1 505
abotics 0:72b953cdcfd1 506 // Find position of return char.
abotics 0:72b953cdcfd1 507 for (index1=1;index1<16;index1++)
abotics 0:72b953cdcfd1 508 {
abotics 0:72b953cdcfd1 509 if (tmpRxBuff[index1] == 13)
abotics 0:72b953cdcfd1 510 {
abotics 0:72b953cdcfd1 511 endPos = index1;
abotics 0:72b953cdcfd1 512 break;
abotics 0:72b953cdcfd1 513 }
abotics 0:72b953cdcfd1 514 }
abotics 0:72b953cdcfd1 515
abotics 0:72b953cdcfd1 516 index2 = 0;
abotics 0:72b953cdcfd1 517 for (index1=1;index1<endPos;index1++)
abotics 0:72b953cdcfd1 518 {
abotics 0:72b953cdcfd1 519 charBuff[index2] = tmpRxBuff[index1];
abotics 0:72b953cdcfd1 520 index2++;
abotics 0:72b953cdcfd1 521 }
abotics 0:72b953cdcfd1 522
abotics 0:72b953cdcfd1 523 gripperPosition = atol(charBuff);
abotics 0:72b953cdcfd1 524 SetGripperPosition(gripperPosition);
abotics 0:72b953cdcfd1 525 }
abotics 0:72b953cdcfd1 526
abotics 0:72b953cdcfd1 527 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 528 // Function to set gripper servo position using supplied pulsewidth[us].
abotics 0:72b953cdcfd1 529 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 530 void Apeiros::SetGripperPosition(int pulseWidth_us)
abotics 0:72b953cdcfd1 531 {
abotics 0:72b953cdcfd1 532 if (pulseWidth_us > MAX_GRIPPER_PULSE) pulseWidth_us = MAX_GRIPPER_PULSE;
abotics 0:72b953cdcfd1 533 if (pulseWidth_us < MIN_GRIPPER_PULSE) pulseWidth_us = MIN_GRIPPER_PULSE;
abotics 0:72b953cdcfd1 534
abotics 0:72b953cdcfd1 535 _gripperPWM.pulsewidth_us(pulseWidth_us);
abotics 0:72b953cdcfd1 536 }