#include <Servo.h>
#include <SoftwareSerial.h>
SoftwareSerial BTserial(52, 53); // RX | TX
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
Servo servo7;
int val;
int val2;
int val3;
int val4;
int val5;
int pos = 90;
int pos2 = 90;
int pos3 = 90;
int pos4 = 90;
int pos5;
int mode = 0;
int n;
int t;
int sw;
char reading;
void setup() {
servo1.attach(9); // attaches the servo on pin 9 to the servo object
servo2.attach(8);
servo3.attach(7);
servo4.attach(6);
servo6.attach(11);
servo5.attach(10);
servo7.attach(5);
pinMode (0, INPUT);
Serial.begin(9600);
BTserial.begin(9600);
pinMode(49, OUTPUT);
sw=0;
}
void loop() {
if (BTserial.available()) mode=1;
if (mode==1){
bluetoothcontrol();
}
else {
manualcontrol();
}
servo1.write(pos); //base rotation
servo2.write(pos2); //arm bottom
servo6.write(180 - pos2);
servo3.write(pos3); //arm top
servo4.write(pos4); //gripper rotation
Serial.print("Joystick X: ");
Serial.print(pos);
Serial.print("\t");
Serial.print(val);
Serial.print("\tY: ");
Serial.print(pos2);
Serial.print("\t");
Serial.print(val2);
Serial.print("\tX2: ");
Serial.print(pos3);
Serial.print("\t");
Serial.print(val4);
Serial.print("\tY2: ");
Serial.print(pos4);
Serial.print("\t");
Serial.print(val5);
Serial.print("\tChwytak: ");
Serial.print(val3);
Serial.print("\treading: ");
Serial.println(reading);
}
void manualcontrol(){
n = analogRead(5);
n = map(n, 0, 1023, 1, 20);
t = (5);
val = analogRead(0); // reads the value of the potentiometer (value between 0 and 1023)
if (val < 450) pos = pos - n;
if (val > 600) pos = pos + n;
if (pos <= 1) pos = 1;
if (pos >= 179)pos = 179;
delay(t);
val2 = analogRead(1);
if (val2 < 450) pos2 = pos2 - n;
if (val2 > 600) pos2 = pos2 + n;
if (pos2 <= 1) pos2 = 1;
if (pos2 >= 179)pos2 = 179;
delay(t);
val4 = analogRead(2);
if (val4 < 450) pos3 = pos3 - n;
if (val4 > 600) pos3 = pos3 + n;
if (pos3 <= 1)pos3 = 1;
if (pos3 >= 179)pos3 = 179;
delay(t);
val5 = analogRead(3);
if (val5 < 450) pos4 = pos4 - n;
if (val5 > 600) pos4 = pos4 + n;
if (pos4 <= 1) pos4 = 1;
if (pos4 >= 179) pos4 = 179;
delay(t);
val3 = analogRead(4);
val3=map(val3, 1023, 0, 180, 0);
delay(20);
servo5.write (val3); //gripper
servo7.write(180 - val3);
}
void bluetoothcontrol(){
reading = BTserial.read();
if (reading=='A') val3=180; //gripper opened
if (reading=='a') val3=0; //gripper closed
if (reading=='B') pos=pos+n; //base left
if (reading=='b') pos=pos-n; //base right
if (reading=='C') pos2=pos2+n; //Arm bottom up
if (reading=='c') pos2=pos2-n; //Arm bottom down
if (reading=='E') pos3=pos3+n; //Arm top up
if (reading=='e') pos3=pos3-n; //Arm top down
if (reading=='D') pos4=pos4+n; //gripper left
if (reading=='d') pos4=pos4-n; //gripper right
servo5.write (val3); //gripper
servo7.write(180 - val3);
if (reading=='n')
{
n=BTserial.parseInt();
}
if (pos <= 1) pos = 1;
if (pos >= 179)pos = 179;
if (pos2 <= 1) pos2 = 1;
if (pos2 >= 179)pos2 = 179;
if (pos3 <= 1) pos3 = 1;
if (pos3 >= 179)pos3 = 179;
if (pos4 <= 1) pos4 = 1;
if (pos4 >= 179)pos4 = 179;
if (pos5 <= 1) pos5 = 1;
if (pos5 >= 179)pos5 = 179;
delay(50);
}
Comments