Skip to content

Instantly share code, notes, and snippets.

@Ajak58a

Ajak58a/omni_directional_PnP_robot.ino

Created April 21, 2024 18:41
Show Gist options
  • Save Ajak58a/e9523685d1dba88e0961dd887b02f230 to your computer and use it in GitHub Desktop.
Save Ajak58a/e9523685d1dba88e0961dd887b02f230 to your computer and use it in GitHub Desktop.
Download ZIP
Robot control program
Raw
omni_directional_PnP_robot.ino
#include <SoftwareSerial.h>
#include <Servo.h>
#define motor_A_1 2
#define motor_A_2 4
#define motor_B_1 7
#define motor_B_2 8
#define motor_C_1 12
#define motor_C_2 13
#define HC12_Tx_pin 19
#define HC12_Rx_pin 18
#define BT_Rx_pin 16
#define BT_Tx_pin 17
#define led 14
#define servo1_centre_position 90
#define servo1_left_position 20
#define servo1_right_position 160
#define servo2_top_position 60
#define servo2_bottom_position 10
#define servo3_top_position 70
#define servo3_bottom_position 110
#define servo4_centre_position 90
#define servo4_left_position 150
#define servo4_right_position 30
#define servo5_centre_position 90
#define servo5_up_position 55
#define servo5_down_position 125
#define servo6_close_position 20
#define servo6_open_position 90
SoftwareSerial HC12_serial(HC12_Rx_pin,HC12_Tx_pin),BT_serial(BT_Rx_pin,BT_Tx_pin);
Servo servo1,servo3,servo2,servo4,servo5,servo6;
int angle1=90,angle2=60,angle3=70,angle4=90,angle5=90,angle6=90;
char BT_ip,serial_ip;
void setup()
{
pinMode(motor_A_1,OUTPUT);
pinMode(motor_A_2,OUTPUT);
pinMode(motor_B_1,OUTPUT);
pinMode(motor_B_2,OUTPUT);
pinMode(motor_C_1,OUTPUT);
pinMode(motor_C_2,OUTPUT);
digitalWrite(motor_A_1,LOW);
digitalWrite(motor_A_2,LOW);
digitalWrite(motor_B_1,LOW);
digitalWrite(motor_B_2,LOW);
digitalWrite(motor_C_1,LOW);
digitalWrite(motor_C_1,LOW);
pinMode(led,OUTPUT);
servo1.attach(3);
servo3.attach(6);
servo2.attach(5);
servo4.attach(9);
servo5.attach(10);
servo6.attach(11);
servo1.write(servo1_centre_position);
servo3.write(servo3_top_position);
servo2.write(servo2_top_position);
servo4.write(servo4_centre_position);
servo5.write(servo5_centre_position);
servo6.write(servo6_open_position);
delay(2000);
HC12_serial.begin(9600);
BT_serial.begin(9600);
for(int t=0;t<5;t++)
{
digitalWrite(led,1);
delay(100);
digitalWrite(led,0);
delay(100);
}
}
void robot_move_forward()
{
digitalWrite(motor_B_1, HIGH);
digitalWrite(motor_C_1, HIGH);
Serial.println("robot moving forward");
delay(2000);
digitalWrite(motor_B_1, LOW);
digitalWrite(motor_C_1, LOW);
}
void robot_move_reverse()
{
digitalWrite(motor_B_2, HIGH);
digitalWrite(motor_C_2, HIGH);
Serial.println("robot moving reverse");
delay(2000);
digitalWrite(motor_B_2, LOW);
digitalWrite(motor_C_2, LOW);
}
void robot_rotate_clockwise()
{
digitalWrite(motor_B_1, HIGH);
digitalWrite(motor_A_1, HIGH);
digitalWrite(motor_C_2, HIGH);
Serial.println("robot rotates clockwise");
delay(500);
digitalWrite(motor_B_1, LOW);
digitalWrite(motor_A_1, LOW);
digitalWrite(motor_C_2, LOW);
}
void robot_rotate_anticlockwise()
{
digitalWrite(motor_B_2, HIGH);
digitalWrite(motor_A_2, HIGH);
digitalWrite(motor_C_1, HIGH);
Serial.println("robot rotates anticlockwise");
delay(500);
digitalWrite(motor_B_2, LOW);
digitalWrite(motor_A_2, LOW);
digitalWrite(motor_C_1, LOW);
}
void robot_move_forward_left()
{
digitalWrite(motor_A_1, HIGH);
digitalWrite(motor_C_1, HIGH);
Serial.println("robot moving forward left");
delay(1000);
digitalWrite(motor_A_1, LOW);
digitalWrite(motor_C_1, LOW);
}
void robot_move_reverse_right()
{
digitalWrite(motor_A_2, HIGH);
digitalWrite(motor_C_2, HIGH);
Serial.println("robot moving reverse right");
delay(1000);
digitalWrite(motor_A_2, LOW);
digitalWrite(motor_C_2, LOW);
}
void robot_move_forward_right()
{
digitalWrite(motor_A_2, HIGH);
digitalWrite(motor_B_1, HIGH);
Serial.println("robot moving forward right");
delay(1000);
digitalWrite(motor_A_2, LOW);
digitalWrite(motor_B_1, LOW);
}
void robot_move_reverse_left()
{
digitalWrite(motor_A_1, HIGH);
digitalWrite(motor_B_2, HIGH);
Serial.println("robot moving reverse left");
delay(1000);
digitalWrite(motor_A_1, LOW);
digitalWrite(motor_B_2, LOW);
}
/////////// base servo motor - servo1 - angle1 - 20 - 160 - left - right/////////////
void base_servo_rightside()
{
if(angle1<servo1_right_position) angle1+=10;
servo1.write(angle1);
}
void base_servo_leftside()
{
if(angle1>servo1_left_position) angle1-=10;
servo1.write(angle1);
}
////////////// upper arm servo - servo2 - angle2 - 60 - 10 - top - bottom /////////////
void upper_arm_servo_down()
{
if(angle2>servo2_bottom_position) angle2-=5;
servo2.write(angle2);
}
void upper_arm_servo_up()
{
if(angle2<servo2_top_position) angle2+=5;
servo2.write(angle2);
}
////////////// lower arm servo - servo3 - angle3 - 70 - 110 - top - bottom /////////////
void lower_arm_servo_down()
{
if(angle3<servo3_bottom_position) angle3+=5;
servo3.write(angle3);
}
void lower_arm_servo_up()
{
if(angle3>servo3_top_position) angle3-=5;
servo3.write(angle3);
}
/////// writst roll servo - servo4 - angle4 - 30 - 150 - left - right ///////////
void wrist_roll_servo_clkwise()
{
if(angle4<servo4_left_position) angle4+=5;
servo4.write(angle4);
}
void wrist_roll_servo_aclkwise()
{
if(angle4>servo4_right_position) angle4-=5;
servo4.write(angle4);
}
/////// wrist yaw servo - servo5 - angle5 - 55 - 125 - up - down ////////////
void wrist_yaw_servo_down()
{
if(angle5<servo5_down_position) angle5+=5;
servo5.write(angle5);
}
void wrist_yaw_servo_up()
{
if(angle5>servo5_up_position) angle5-=5;
servo5.write(angle5);
}
////////// grip servo - servo6 - angle6 - 90 - 20 - open - close //////////
void grip_servo_open()
{
if(angle6<servo6_open_position) angle6+=2;
servo6.write(angle6);
}
void grip_servo_close()
{
if(angle6>servo6_close_position) angle6-=2;
servo6.write(angle6);
}
void loop()
{
while(BT_serial.available() > 0)
{
digitalWrite(led,1);
BT_ip = BT_serial.read();
if (BT_ip == 'W') robot_move_forward();
else if (BT_ip == 'X') robot_move_reverse();
else if (BT_ip == 'D') robot_rotate_clockwise();
else if (BT_ip == 'A') robot_rotate_anticlockwise();
else if (BT_ip == 'Q') robot_move_forward_left();
else if (BT_ip == 'C') robot_move_reverse_right();
else if (BT_ip == 'E') robot_move_forward_right();
else if (BT_ip == 'Z') robot_move_reverse_left();
else if (BT_ip == 'P') base_servo_rightside();
else if (BT_ip == 'O') base_servo_leftside();
else if (BT_ip == 'I') upper_arm_servo_down();
else if (BT_ip == 'U') upper_arm_servo_up();
else if (BT_ip == 'Y') lower_arm_servo_down();
else if (BT_ip == 'T') lower_arm_servo_up();
else if (BT_ip == 'R') wrist_roll_servo_clkwise();
else if (BT_ip == 'L') wrist_roll_servo_aclkwise();
else if (BT_ip == 'K') wrist_yaw_servo_down();
else if (BT_ip == 'J') wrist_yaw_servo_up();
else if (BT_ip == 'G') grip_servo_open();
else if (BT_ip == 'H') grip_servo_close ();
digitalWrite(led,0);
}
while(HC12_serial.available() > 0)
{
digitalWrite(led,1);
serial_ip = HC12_serial.read();
if (serial_ip == 'W') robot_move_forward();
else if (serial_ip == 'X') robot_move_reverse();
else if (serial_ip == 'D') robot_rotate_clockwise();
else if (serial_ip == 'A') robot_rotate_anticlockwise();
else if (serial_ip == 'Q') robot_move_forward_left();
else if (serial_ip == 'C') robot_move_reverse_right();
else if (serial_ip == 'E') robot_move_forward_right();
else if (serial_ip == 'Z') robot_move_reverse_left();
else if (serial_ip == 'P') base_servo_rightside();
else if (serial_ip == 'O') base_servo_leftside();
else if (serial_ip == 'I') upper_arm_servo_down();
else if (serial_ip == 'U') upper_arm_servo_up();
else if (serial_ip == 'Y') lower_arm_servo_down();
else if (serial_ip == 'T') lower_arm_servo_up();
else if (serial_ip == 'R') wrist_roll_servo_clkwise();
else if (serial_ip == 'L') wrist_roll_servo_aclkwise();
else if (serial_ip == 'K') wrist_yaw_servo_down();
else if (serial_ip == 'J') wrist_yaw_servo_up();
else if (serial_ip== 'G') grip_servo_open();
else if (serial_ip== 'H') grip_servo_close ();
digitalWrite(led,0);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment