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 19 21:30:39 2014 +0000
Revision:
1:8edccfd4646a
Parent:
0:72b953cdcfd1
Child:
3:c61c3feb0729
Updated buzzer function in order to resolve a bug. Added support for wheel encoders by creating a Get function for a connected left & right encoder.

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 1:8edccfd4646a 52 buzzerPeriod = 0;
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 1:8edccfd4646a 308 {
abotics 1:8edccfd4646a 309 if (buzzerTone != buzzerPeriod)
abotics 0:72b953cdcfd1 310 {
abotics 1:8edccfd4646a 311 if (buzzerTone > 0)
abotics 1:8edccfd4646a 312 {
abotics 1:8edccfd4646a 313 if (buzzerTone > MAX_BUZZER_PWM) buzzerTone = MAX_BUZZER_PWM;
abotics 1:8edccfd4646a 314 buzzerTick = 1;
abotics 1:8edccfd4646a 315 buzzerPeriod = buzzerTone;
abotics 1:8edccfd4646a 316 buzzerDuty = buzzerPeriod/2;
abotics 1:8edccfd4646a 317 enableBuzzer = true;
abotics 1:8edccfd4646a 318 }
abotics 1:8edccfd4646a 319 else
abotics 1:8edccfd4646a 320 {
abotics 1:8edccfd4646a 321 buzzerPeriod = 0;
abotics 1:8edccfd4646a 322 enableBuzzer = false;
abotics 1:8edccfd4646a 323 _buzzerOut = 0;
abotics 1:8edccfd4646a 324 }
abotics 0:72b953cdcfd1 325 }
abotics 0:72b953cdcfd1 326 }
abotics 0:72b953cdcfd1 327
abotics 0:72b953cdcfd1 328 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 329 // Calculate Wheel Speeds
abotics 0:72b953cdcfd1 330 //
abotics 0:72b953cdcfd1 331 // Speed in 0.1"/sec = (Cwh / NCLKS) * TSCALE / (TICK * PER) = Ktps / PER
abotics 0:72b953cdcfd1 332 // where Cwh = 8.12" wheel circumference, NCLKS = 128 (32 stripe disk),
abotics 0:72b953cdcfd1 333 // TSCALE = 10 (to convert inches to 0.1"), TICK = 205us per timer2 tick,
abotics 0:72b953cdcfd1 334 // and PER is the measured period in timer 2 ticks
abotics 0:72b953cdcfd1 335 // Ktps = 3098
abotics 0:72b953cdcfd1 336 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 337 void Apeiros::CalculateWheelSpeed()
abotics 0:72b953cdcfd1 338 {
abotics 0:72b953cdcfd1 339 // If the wheel is spinning so slow that we don't have current reading.
abotics 0:72b953cdcfd1 340 if (!encoderUpdated_R) encoderPeriod_R = motorUpdateTickCount - prevT3Count_R; // Calculate period since last update.
abotics 0:72b953cdcfd1 341 else encoderUpdated_R = false; // Otherwise use it.
abotics 0:72b953cdcfd1 342
abotics 0:72b953cdcfd1 343 // Converts from 205us ticks per edge to multiples of 0.1 inches per second.
abotics 0:72b953cdcfd1 344 encoderSpeed_R = (signed short)(Ktps / encoderPeriod_R);
abotics 0:72b953cdcfd1 345
abotics 0:72b953cdcfd1 346 // Check direction of wheel rotation & adjust as needed. */
abotics 0:72b953cdcfd1 347 if (!rightEncoderDir) encoderSpeed_R = -encoderSpeed_R;
abotics 0:72b953cdcfd1 348
abotics 0:72b953cdcfd1 349 if (!encoderUpdated_L) encoderPeriod_L = motorUpdateTickCount - prevT3Count_L;
abotics 0:72b953cdcfd1 350 else encoderUpdated_L = false;
abotics 0:72b953cdcfd1 351
abotics 0:72b953cdcfd1 352 // Converts from 205us ticks per edge to multiples of 0.1 inches per second
abotics 0:72b953cdcfd1 353 encoderSpeed_L = (signed short)(Ktps / encoderPeriod_L);
abotics 0:72b953cdcfd1 354
abotics 0:72b953cdcfd1 355 // Check direction of wheel rotation & adjust as needed. */
abotics 0:72b953cdcfd1 356 if (leftEncoderDir) encoderSpeed_L = -encoderSpeed_L;
abotics 0:72b953cdcfd1 357 }
abotics 0:72b953cdcfd1 358
abotics 0:72b953cdcfd1 359 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 360 // Function to set left & right motor speed and direction.
abotics 0:72b953cdcfd1 361 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 362 void Apeiros::SetMotorSpeed(int leftMotorSpeed, int rightMotorSpeed)
abotics 0:72b953cdcfd1 363 {
abotics 0:72b953cdcfd1 364
abotics 0:72b953cdcfd1 365 int tmpLeftSpeed = 0;
abotics 0:72b953cdcfd1 366 int tmpRightSpeed = 0;
abotics 0:72b953cdcfd1 367
abotics 0:72b953cdcfd1 368 if (rightMotorSpeed < 0)
abotics 0:72b953cdcfd1 369 {
abotics 0:72b953cdcfd1 370 _SN_2A = 0;
abotics 0:72b953cdcfd1 371 _SN_1A = 1;
abotics 0:72b953cdcfd1 372 }
abotics 0:72b953cdcfd1 373 else {
abotics 0:72b953cdcfd1 374 _SN_2A = 1;
abotics 0:72b953cdcfd1 375 _SN_1A = 0; }
abotics 0:72b953cdcfd1 376
abotics 0:72b953cdcfd1 377 if (leftMotorSpeed < 0)
abotics 0:72b953cdcfd1 378 {
abotics 0:72b953cdcfd1 379 _SN_3A = 0;
abotics 0:72b953cdcfd1 380 _SN_4A = 1;
abotics 0:72b953cdcfd1 381 }
abotics 0:72b953cdcfd1 382 else {
abotics 0:72b953cdcfd1 383 _SN_3A = 1;
abotics 0:72b953cdcfd1 384 _SN_4A = 0; }
abotics 0:72b953cdcfd1 385
abotics 0:72b953cdcfd1 386 if (leftMotorSpeed != 0) tmpLeftSpeed = abs(leftMotorSpeed) + leftMotorOffset;
abotics 0:72b953cdcfd1 387 else
abotics 0:72b953cdcfd1 388 {
abotics 0:72b953cdcfd1 389 tmpLeftSpeed = 0;
abotics 0:72b953cdcfd1 390 _SN_3A = _SN_4A = 0;
abotics 0:72b953cdcfd1 391 }
abotics 0:72b953cdcfd1 392
abotics 0:72b953cdcfd1 393 if (rightMotorSpeed != 0) tmpRightSpeed = abs(rightMotorSpeed) + rightMotorOffset;
abotics 0:72b953cdcfd1 394 else
abotics 0:72b953cdcfd1 395 {
abotics 0:72b953cdcfd1 396 tmpRightSpeed = 0;
abotics 0:72b953cdcfd1 397 _SN_1A = _SN_2A = 0;
abotics 0:72b953cdcfd1 398 }
abotics 0:72b953cdcfd1 399
abotics 0:72b953cdcfd1 400 if (tmpLeftSpeed > MaxMotorSpeed) tmpLeftSpeed = MaxMotorSpeed;
abotics 0:72b953cdcfd1 401 if (tmpRightSpeed > MaxMotorSpeed) tmpRightSpeed = MaxMotorSpeed;
abotics 0:72b953cdcfd1 402
abotics 0:72b953cdcfd1 403 _leftMotorPWM.pulsewidth_us(tmpLeftSpeed);
abotics 0:72b953cdcfd1 404 _rightMotorPWM.pulsewidth_us(tmpRightSpeed);
abotics 0:72b953cdcfd1 405
abotics 0:72b953cdcfd1 406 }
abotics 0:72b953cdcfd1 407
abotics 0:72b953cdcfd1 408 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 409 // Function to parse motor commands from UART data.
abotics 0:72b953cdcfd1 410 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 411 void Apeiros::ParseMotorCommand()
abotics 0:72b953cdcfd1 412 {
abotics 0:72b953cdcfd1 413 unsigned int commaPos, endPos, index1, index2;
abotics 0:72b953cdcfd1 414 signed short leftWheel, rightWheel;
abotics 0:72b953cdcfd1 415 char charBuff[5];
abotics 0:72b953cdcfd1 416 bool foundNegative = false;
abotics 0:72b953cdcfd1 417
abotics 0:72b953cdcfd1 418 commaPos = 0;
abotics 0:72b953cdcfd1 419 endPos = 0;
abotics 0:72b953cdcfd1 420
abotics 0:72b953cdcfd1 421 // Find comma position and return char.
abotics 0:72b953cdcfd1 422 for (index1=2;index1<16;index1++)
abotics 0:72b953cdcfd1 423 {
abotics 0:72b953cdcfd1 424 if (tmpRxBuff[index1] == ',') commaPos = index1;
abotics 0:72b953cdcfd1 425 else if (tmpRxBuff[index1] == 13)
abotics 0:72b953cdcfd1 426 {
abotics 0:72b953cdcfd1 427 endPos = index1;
abotics 0:72b953cdcfd1 428 break;
abotics 0:72b953cdcfd1 429 }
abotics 0:72b953cdcfd1 430 }
abotics 0:72b953cdcfd1 431
abotics 0:72b953cdcfd1 432 // Parse out left motor data.
abotics 0:72b953cdcfd1 433 for (index1=0;index1<5;index1++) charBuff[index1] = ' ';
abotics 0:72b953cdcfd1 434 index2 = 0;
abotics 0:72b953cdcfd1 435 for (index1=2;index1<commaPos;index1++)
abotics 0:72b953cdcfd1 436 {
abotics 0:72b953cdcfd1 437 if (tmpRxBuff[index1] != '-') charBuff[index2] = tmpRxBuff[index1];
abotics 0:72b953cdcfd1 438 else foundNegative = true;
abotics 0:72b953cdcfd1 439 index2++;
abotics 0:72b953cdcfd1 440 }
abotics 0:72b953cdcfd1 441 leftWheel = atol(charBuff);
abotics 0:72b953cdcfd1 442 if (foundNegative) leftWheel = -leftWheel;
abotics 0:72b953cdcfd1 443
abotics 0:72b953cdcfd1 444 foundNegative = false;
abotics 0:72b953cdcfd1 445
abotics 0:72b953cdcfd1 446 // Parse out right motor data.
abotics 0:72b953cdcfd1 447 for (index1=0;index1<5;index1++) charBuff[index1] = ' ';
abotics 0:72b953cdcfd1 448 index2 = 0;
abotics 0:72b953cdcfd1 449 for (index1=commaPos+1;index1<endPos;index1++)
abotics 0:72b953cdcfd1 450 {
abotics 0:72b953cdcfd1 451 if (tmpRxBuff[index1] != '-') charBuff[index2] = tmpRxBuff[index1];
abotics 0:72b953cdcfd1 452 else foundNegative = true;
abotics 0:72b953cdcfd1 453 index2++;
abotics 0:72b953cdcfd1 454 }
abotics 0:72b953cdcfd1 455 rightWheel = atol(charBuff);
abotics 0:72b953cdcfd1 456 if (foundNegative) rightWheel = -rightWheel;
abotics 0:72b953cdcfd1 457
abotics 0:72b953cdcfd1 458 SetMotorSpeed(leftWheel, rightWheel);
abotics 0:72b953cdcfd1 459 }
abotics 0:72b953cdcfd1 460
abotics 0:72b953cdcfd1 461 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 462 // Function to parse piezo buzzer tone commands from UART data.
abotics 0:72b953cdcfd1 463 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 464 void Apeiros::ParseBuzzerCommand()
abotics 0:72b953cdcfd1 465 {
abotics 0:72b953cdcfd1 466 unsigned int index1, index2, endPos;
abotics 0:72b953cdcfd1 467 unsigned int buzzerTonePWM;
abotics 0:72b953cdcfd1 468 char charBuff[3];
abotics 0:72b953cdcfd1 469
abotics 0:72b953cdcfd1 470 index1 = index2 = endPos = 0;
abotics 0:72b953cdcfd1 471 buzzerTonePWM = 1;
abotics 0:72b953cdcfd1 472
abotics 0:72b953cdcfd1 473 for (index1=0;index1<3;index1++) charBuff[index1] = ' ';
abotics 0:72b953cdcfd1 474
abotics 0:72b953cdcfd1 475 // Find position of return char.
abotics 0:72b953cdcfd1 476 for (index1=1;index1<16;index1++)
abotics 0:72b953cdcfd1 477 {
abotics 0:72b953cdcfd1 478 if (tmpRxBuff[index1] == 13)
abotics 0:72b953cdcfd1 479 {
abotics 0:72b953cdcfd1 480 endPos = index1;
abotics 0:72b953cdcfd1 481 break;
abotics 0:72b953cdcfd1 482 }
abotics 0:72b953cdcfd1 483 }
abotics 0:72b953cdcfd1 484
abotics 0:72b953cdcfd1 485 index2 = 0;
abotics 0:72b953cdcfd1 486 for (index1=1;index1<endPos;index1++)
abotics 0:72b953cdcfd1 487 {
abotics 0:72b953cdcfd1 488 charBuff[index2] = tmpRxBuff[index1];
abotics 0:72b953cdcfd1 489 index2++;
abotics 0:72b953cdcfd1 490 }
abotics 0:72b953cdcfd1 491
abotics 0:72b953cdcfd1 492 buzzerTonePWM = atol(charBuff);
abotics 0:72b953cdcfd1 493 SetBuzzerTone(buzzerTonePWM);
abotics 0:72b953cdcfd1 494 }
abotics 0:72b953cdcfd1 495
abotics 0:72b953cdcfd1 496 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 497 // Function to parse gripper servo commands from UART data.
abotics 0:72b953cdcfd1 498 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 499 void Apeiros::ParseGripperCommand()
abotics 0:72b953cdcfd1 500 {
abotics 0:72b953cdcfd1 501 unsigned int index1, index2, endPos;
abotics 0:72b953cdcfd1 502 unsigned int gripperPosition;
abotics 0:72b953cdcfd1 503 char charBuff[4];
abotics 0:72b953cdcfd1 504
abotics 0:72b953cdcfd1 505 index1 = index2 = endPos = 0;
abotics 0:72b953cdcfd1 506 gripperPosition = 2000;
abotics 0:72b953cdcfd1 507
abotics 0:72b953cdcfd1 508 for (index1=0;index1<4;index1++) charBuff[index1] = ' ';
abotics 0:72b953cdcfd1 509
abotics 0:72b953cdcfd1 510 // Find position of return char.
abotics 0:72b953cdcfd1 511 for (index1=1;index1<16;index1++)
abotics 0:72b953cdcfd1 512 {
abotics 0:72b953cdcfd1 513 if (tmpRxBuff[index1] == 13)
abotics 0:72b953cdcfd1 514 {
abotics 0:72b953cdcfd1 515 endPos = index1;
abotics 0:72b953cdcfd1 516 break;
abotics 0:72b953cdcfd1 517 }
abotics 0:72b953cdcfd1 518 }
abotics 0:72b953cdcfd1 519
abotics 0:72b953cdcfd1 520 index2 = 0;
abotics 0:72b953cdcfd1 521 for (index1=1;index1<endPos;index1++)
abotics 0:72b953cdcfd1 522 {
abotics 0:72b953cdcfd1 523 charBuff[index2] = tmpRxBuff[index1];
abotics 0:72b953cdcfd1 524 index2++;
abotics 0:72b953cdcfd1 525 }
abotics 0:72b953cdcfd1 526
abotics 0:72b953cdcfd1 527 gripperPosition = atol(charBuff);
abotics 0:72b953cdcfd1 528 SetGripperPosition(gripperPosition);
abotics 0:72b953cdcfd1 529 }
abotics 0:72b953cdcfd1 530
abotics 0:72b953cdcfd1 531 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 532 // Function to set gripper servo position using supplied pulsewidth[us].
abotics 0:72b953cdcfd1 533 //------------------------------------------------------------------------
abotics 0:72b953cdcfd1 534 void Apeiros::SetGripperPosition(int pulseWidth_us)
abotics 0:72b953cdcfd1 535 {
abotics 0:72b953cdcfd1 536 if (pulseWidth_us > MAX_GRIPPER_PULSE) pulseWidth_us = MAX_GRIPPER_PULSE;
abotics 0:72b953cdcfd1 537 if (pulseWidth_us < MIN_GRIPPER_PULSE) pulseWidth_us = MIN_GRIPPER_PULSE;
abotics 0:72b953cdcfd1 538
abotics 0:72b953cdcfd1 539 _gripperPWM.pulsewidth_us(pulseWidth_us);
abotics 1:8edccfd4646a 540 }
abotics 1:8edccfd4646a 541
abotics 1:8edccfd4646a 542 //------------------------------------------------------------------------
abotics 1:8edccfd4646a 543 // Function to return left wheel encoder count [integer].
abotics 1:8edccfd4646a 544 //------------------------------------------------------------------------
abotics 1:8edccfd4646a 545 int Apeiros::GetLeftEncoder()
abotics 1:8edccfd4646a 546 {
abotics 1:8edccfd4646a 547 return leftEncoderCount;
abotics 1:8edccfd4646a 548 }
abotics 1:8edccfd4646a 549
abotics 1:8edccfd4646a 550 //------------------------------------------------------------------------
abotics 1:8edccfd4646a 551 // Function to return left wheel encoder count [integer].
abotics 1:8edccfd4646a 552 //------------------------------------------------------------------------
abotics 1:8edccfd4646a 553 int Apeiros::GetRightEncoder()
abotics 1:8edccfd4646a 554 {
abotics 1:8edccfd4646a 555 return rightEncoderCount;
abotics 1:8edccfd4646a 556 }
abotics 1:8edccfd4646a 557
abotics 1:8edccfd4646a 558 //------------------------------------------------------------------------
abotics 1:8edccfd4646a 559 // Function to reset both the left & right wheel encoder counts.
abotics 1:8edccfd4646a 560 //------------------------------------------------------------------------
abotics 1:8edccfd4646a 561 void Apeiros::ResetWheelEncoders()
abotics 1:8edccfd4646a 562 {
abotics 1:8edccfd4646a 563 leftEncoderCount = 0;
abotics 1:8edccfd4646a 564 rightEncoderCount = 0;
abotics 0:72b953cdcfd1 565 }