Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 3:c61c3feb0729, committed 2014-12-23
- Comitter:
- abotics
- Date:
- Tue Dec 23 23:29:00 2014 +0000
- Parent:
- 2:71ede4f26f71
- Commit message:
- 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.
Changed in this revision
| Apeiros.cpp | Show annotated file Show diff for this revision Revisions of this file |
| Apeiros.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/Apeiros.cpp Tue Dec 16 17:54:27 2014 +0000
+++ b/Apeiros.cpp Tue Dec 23 23:29:00 2014 +0000
@@ -5,7 +5,7 @@
// Constructor for Apeiros Class.
//------------------------------------------------------------------------
Apeiros::Apeiros(PinName tx, PinName rx, int leftMotorPwmOffset, int rightMotorPwmOffset) : Serial(tx, rx), leftFrontIR(PC_15),centerFrontIR(PC_14),rightFrontIR(PC_13),
-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),
+leftRearIR(PA_13), rightRearIR(PA_14),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),
_leftEncoderDirPin(PA_6),_rightEncoderDirPin(PA_7),_leftEncoderClk(PH_1),_rightEncoderClk(PH_0),_rightMotorPWM(PB_10),_leftMotorPWM(PB_4),_gripperPWM(PB_6)
{
// Set direction of PWM dir pins to be low so robot is halted. //
@@ -246,7 +246,7 @@
switch (tmpRxBuff[0]){
case 'A':
- 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]);
+ printf("Sensors = (%0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f)\r\n", adSensors[0], adSensors[1], adSensors[2], adSensors[3], adSensors[4], adSensors[5]);
break;
case 'B':
@@ -255,7 +255,7 @@
break;
case 'D':
- printf("Sensors = (%d, %d, %d)\r\n", leftFrontIR.read(), centerFrontIR.read(), rightFrontIR.read());
+ printf("Sensors = (%d, %d, %d, %d, %d)\r\n", leftFrontIR.read(), centerFrontIR.read(), rightFrontIR.read(),leftRearIR.read(),rightRearIR.read());
break;
case 'E':
--- a/Apeiros.h Tue Dec 16 17:54:27 2014 +0000
+++ b/Apeiros.h Tue Dec 23 23:29:00 2014 +0000
@@ -6,7 +6,7 @@
* #include "mbed.h"
* #include "Apeiros.h"
*
- * Apeiros apeiros(SERIAL_TX, SERIAL_RX, 120, 120);
+ * Apeiros apeiros(SERIAL_TX, SERIAL_RX, 100, 100);
*
* int main() {
* apeiros.Begin();
@@ -32,8 +32,10 @@
#define Ktps ((Cwh * TSCALE * INVTICK) / NCLKS)
#define MaxMotorSpeed 255
+// Hitec HS-81 accepts a 600 to 2400us pulse control signal. //
+// Do not use a value that is outside of the HS-81 specification! //
#define MIN_GRIPPER_PULSE 1000
-#define MAX_GRIPPER_PULSE 2300
+#define MAX_GRIPPER_PULSE 2200
#define MAX_BUZZER_PWM 100
@@ -49,10 +51,17 @@
/** Create Apeiros instance
*
- * @param tx specified as PinName of UART Tx pin. Use SERIAL_TX as default.
- * @param rx specified as PinName of UART Rx pin. Use SERIAL_RX as deafult.
- * @param leftMotorPwmOffset specified as integer value from 0-150. Use 120 as default.
- * @param rightMotorPwmOffset specified as integer value from 0-150. Use 120 as default.
+ * @param tx specified as PinName of UART Tx pin. Use SERIAL_TX as default. Use PA_9 for wireless shield.
+ * @param rx specified as PinName of UART Rx pin. Use SERIAL_RX as deafult. Use PA_10 for wireless shield.
+ * @param leftMotorPwmOffset specified as integer value from 0-150. Use 100 as default.
+ * @param rightMotorPwmOffset specified as integer value from 0-150. Use 100 as default.
+ *
+ * The PwmOffsets are used to eliminate the deadband for both the left & right motor, so
+ * that each motor begins to rotate when motor speeds are specified as aperiros.SetMotorSpeed(1,1).
+ *
+ * Pass a zero for each PwmOffset parameter and then use SetMotorSpeed function to find the leftMotorPwmOffset & rightMotorPwmOffset values.
+ *
+ * You have found the PwmOffset value when the respective motor goes from not rotating to starting rotation.
*/
Apeiros(PinName tx, PinName rx, int leftMotorPwmOffset, int rightMotorPwmOffset);
@@ -80,15 +89,26 @@
*/
void SetBuzzerTone(int buzzerTone);
- /** Set left and roght motor speeds.
+ /** Set left and right motor speeds.
+ *
+ * @param leftMotorSpeed specified as an integer value(int) from 0 to (255-leftMotorPwmOffset). Use a preceeding negative sign to reverse motor rotation.
+ * @param rightMotorSpeed specified as an integer value(int) from 0 to (255-rightMotorPwmOffset). Use a preceeding negative sign to reverse motor rotation.
+ *
+ * For example, if leftMotorPwmOffset = 95 then valid left motor speed range is 0 to 165. Use a preceeding negative sign to reverse motor rotation.
*
- * @param leftMotorSpeed specified as an integer value(int) from 0-150, rightMotorSpeed specified as an integer value(int) from 0-150.
+ * Drive robot forward with apeiros.SetMotorSpeed(65,65).
+ *
+ * Drive robot in reverse with apeiros.SetMotorSpeed(-65,-65).
+ *
+ * Spin robot clockwise with apeiros.SetMotorSpeed(65,-65).
+ *
+ * Spin robot counterclockwise with apeiros.SetMotorSpeed(-65,65).
*/
void SetMotorSpeed(int leftMotorSpeed, int rightMotorSpeed);
/** Set servo gripper position.
*
- * @param pulseWidth_us specified as an integer value(int) from MIN_GRIPPER_PULSE(1000) to MAX_GRIPPER_PULSE(2300).
+ * @param pulseWidth_us specified as an integer value(int) from MIN_GRIPPER_PULSE(1000) to MAX_GRIPPER_PULSE(2200).
*/
void SetGripperPosition(int pulseWidth_us);
@@ -111,15 +131,54 @@
*/
void ResetWheelEncoders(void);
+ /** DigitalIn object for Left Front IR Sensor.
+ *
+ */
DigitalIn leftFrontIR;
+
+ /** DigitalIn object for Center Front IR Sensor.
+ *
+ */
DigitalIn centerFrontIR;
+
+ /** DigitalIn object for Right Front IR Sensor.
+ *
+ */
DigitalIn rightFrontIR;
+ /** DigitalIn object for Left Rear IR Sensor.
+ *
+ */
+ DigitalIn leftRearIR;
+
+ /** DigitalIn object for Right Rear IR Sensor.
+ *
+ */
+ DigitalIn rightRearIR;
+
+ /** ad_0 AnalogIn object. Used to convert a varying voltage to a digital value.
+ *
+ */
AnalogIn ad_0;
+ /** ad_1 AnalogIn object. Used to convert a varying voltage to a digital value.
+ *
+ */
AnalogIn ad_1;
+ /** ad_2 AnalogIn object. Used to convert a varying voltage to a digital value.
+ *
+ */
AnalogIn ad_2;
+ /** ad_3 AnalogIn object. Used to convert a varying voltage to a digital value.
+ *
+ */
AnalogIn ad_3;
+ /** ad_4 AnalogIn object. Used to convert a varying voltage to a digital value.
+ *
+ */
AnalogIn ad_4;
+ /** ad_5 AnalogIn object. Used to convert a varying voltage to a digital value.
+ *
+ */
AnalogIn ad_5;