#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
// Define the PCA9685 address and PWM frequency
#define PCA9685_ADDRESS 0x40
#define PWM_FREQUENCY 50
#define SERVO_MIN 150 //150
#define SERVO_MAX 600 //600
//below for continuous leg servos
#define SERVO_FORWARD 125 //1000ms 125
#define SERVO_STOP 375 //1500ms 315
#define SERVO_REVERSE 450 //2000ms 405
#define SERVOPINLT 4 //left leg
#define SERVOPINRT 5 //right leg
// Create a PCA9685 object
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(PCA9685_ADDRESS);
//I2C communication
DFRobot_DF2301Q_I2C asr;
//Arduino mega 2560
void setup() {
Serial.begin(115200); //enable if plugged into pc to troubleshoot code
Serial.println("Servo control with PCA9685");
pwm.begin();
pwm.setPWMFreq(PWM_FREQUENCY);
//enable voice recognition
while (!(asr.begin())) {
Serial.println("Communication with Device failed, please check connection");
delay(3000);
}
Serial.println("Begin ok!");
asr.setVolume(7); //value(1-7)
asr.setMuteMode(0); //Mute value 1, unmute value 0
asr.setWakeTime(30); //Wake duration (0-255)
//below is for voice module dont edit
uint8_t wakeTime = 0;
wakeTime = asr.getWakeTime();
Serial.print("wakeTime = ");
Serial.println(wakeTime);
//asr.playByCMDID(1); //Wake-up command
//asr.playByCMDID(20); //Command word ID
}
void rotateServo(uint16_t pulseLength) {
pwm.setPWM(SERVOPINLT, 0, pulseLength);
pwm.setPWM(SERVOPINRT, 0, pulseLength);
}
void loop() {
uint8_t CMDID = asr.getCMDID();
switch (CMDID) {
case 5: //Custom command "Left Arm Up"
for (uint16_t pulseLength = SERVO_MAX; pulseLength > SERVO_MIN; pulseLength--) {
pwm.setPWM(0, 0, pulseLength); // Channel 0, 0 for off, pulselen for on
delay(5); //speed for action
}
Serial.println("received 'Left Arm Up',command flag '5'");
delay(100);
break;
case 6: //Custom command "Left Arm Down"
for (uint16_t pulseLength = SERVO_MIN; pulseLength < SERVO_MAX; pulseLength++) {
pwm.setPWM(0, 0, pulseLength);
delay(5);
}
Serial.println("received 'Left Arm Down',command flag '6'");
break;
case 7: //Custom command "Right Arm Up"
for (uint16_t pulseLength = SERVO_MIN; pulseLength < SERVO_MAX; pulseLength++) {
pwm.setPWM(2, 0, pulseLength); // Channel 0, 0 for off, pulselen for on
delay(5); //speed for action
}
Serial.println("received 'Right Arm Up',command flag '7'");
delay(100);
break;
case 8: //Custom command "Right Arm Down"
for (uint16_t pulseLength = SERVO_MAX; pulseLength > SERVO_MIN; pulseLength--) {
pwm.setPWM(2, 0, pulseLength);
delay(5);
}
Serial.println("received 'Right Arm Down',command flag '8'");
delay(100);
break;
case 9: //Custom command "Left Gripper Open" fixx this not working command
for (uint16_t pulseLength = SERVO_MIN; pulseLength < SERVO_MAX; pulseLength++) {
pwm.setPWM(1, 0, pulseLength); // Channel ?, 0 for off, pulselen for on
delay(1); //speed for action
}
Serial.println("received 'Left Gripper Open',command flag '9'");
delay(100);
break;
case 10: //Custom command "Left Gripper Close"
for (uint16_t pulseLength = SERVO_MAX; pulseLength > SERVO_MIN; pulseLength--) {
pwm.setPWM(1, 0, pulseLength);
delay(1);
}
Serial.println("received 'Left Gripper Close',command flag '10'");
delay(100);
break;
case 22: //Fixed command "Go Forward"
pwm.setPWM(SERVOPINLT, 0, SERVO_REVERSE);
pwm.setPWM(SERVOPINRT, 0, SERVO_FORWARD);
delay(1000);
Serial.println("received 'Go Forward',command flag '22'");
pwm.setPWM(SERVOPINLT, 0, 1500);
pwm.setPWM(SERVOPINRT, 0, 1500);
delay(100);
break;
case 23: ////Fixed command "Retreat"
pwm.setPWM(SERVOPINLT, 0, SERVO_FORWARD);
pwm.setPWM(SERVOPINRT, 0, SERVO_REVERSE);
delay(1000);
Serial.println("received 'Retreat',command flag '23'");
pwm.setPWM(SERVOPINLT, 0, 1500);
pwm.setPWM(SERVOPINRT, 0, 1500);
delay(100);
break;
case 25: //Fixed command "Turn Left 90degs"
pwm.setPWM(SERVOPINLT, 0, SERVO_FORWARD);
pwm.setPWM(SERVOPINRT, 0, SERVO_REVERSE);
delay(2000);
Serial.println("received 'Turn Left 90degs',command flag '25'");
pwm.setPWM(SERVOPINLT, 1500, 1500);
pwm.setPWM(SERVOPINRT, 1500, 1500);
delay(100);
break;
case 26: //Fixed command "Turn Left 45degs"
pwm.setPWM(SERVOPINLT, 0, SERVO_FORWARD);
pwm.setPWM(SERVOPINRT, 0, SERVO_REVERSE);
delay(1000);
Serial.println("received 'Turn Left 45degs',command flag '26'");
pwm.setPWM(SERVOPINLT, 1500, 1500);
pwm.setPWM(SERVOPINRT, 1500, 1500);
delay(100);
break;
case 27: //Fixed command "Turn Left 30degs"
pwm.setPWM(SERVOPINLT, 0, SERVO_FORWARD);
pwm.setPWM(SERVOPINRT, 0, SERVO_REVERSE);
delay(1000);
Serial.println("received 'Turn Left 30degs',command flag '27'");
pwm.setPWM(SERVOPINLT, 1500, 1500);
pwm.setPWM(SERVOPINRT, 1500, 1500);
delay(100);
break;
case 29: //Fixed command "Turn Right 45degs"
pwm.setPWM(SERVOPINLT, 0, SERVO_FORWARD);
pwm.setPWM(SERVOPINRT, 0, SERVO_REVERSE);
delay(1000);
Serial.println("received 'Turn Right 45degs',command flag '29'");
pwm.setPWM(SERVOPINLT, 1500, 1500);
pwm.setPWM(SERVOPINRT, 1500, 1500);
delay(100);
break;
case 30: //Fixed command "Turn Right 30degs"
pwm.setPWM(SERVOPINLT, 0, SERVO_FORWARD);
pwm.setPWM(SERVOPINRT, 0, SERVO_REVERSE);
delay(1000);
Serial.println("received 'Turn Right 30degs ',command flag '30'");
pwm.setPWM(SERVOPINLT, 1500, 1500);
pwm.setPWM(SERVOPINRT, 1500, 1500);
delay(100);
break;
default:
if (CMDID != 0) {
Serial.print("CMDID = "); //Printing command ID
Serial.println(CMDID);
}
//if (case 25){ pwm.setPWM(3, 1500, 1600 );
//delay(1000); rotate head in direction to command (testing not in use)
delay(300);
}
}
type or paste code here