Created
April 21, 2024 18:41
-
-
Save Ajak58a/e9523685d1dba88e0961dd887b02f230 to your computer and use it in GitHub Desktop.
Robot control program
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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