Hướng dẫn lập trình cánh tay robot

Đây là topic cho thử thách của chương trình
Học lập trình ở đâu


Và dưới này thì có code mẫu nhé
Code nạp vào Arduino trên tay robot


#include

Servo khop1; //0-180
Servo khop2; //0-80-150
Servo khop3; //0-100
Servo khop4; //0-100

int bluetoothByte = 0;

void setup[]
{
Serial.begin [9600];
khop1.attach[2];//0-180
khop2.attach[3];//0-80-150
khop3.attach[4];//0-100
khop4.attach[5];//0-100
}

void loop[]
{
bluetoothRobotArm[];
}

void bluetoothRobotArm[]
{
if [Serial.available[] > 0]
bluetoothByte = Serial.read[];
switch [bluetoothByte]
{
case '0':
stopArm[];
break;
case '1':
vuonRa1Buoc[];
break;
case '2':
thuVao1Buoc[];
break;
case '3':
quayTrai1Buoc[];
break;
case '4':
quayPhai1Buoc[];
break;
case '5':
nangLen1Buoc[];
break;
case '6':
haXuong1Buoc[];
break;
case '7':
kep1Buoc[];
break;
case '8':
moRa1Buoc[];
break;


}
}

void stopArm[]
{
servoPosition[khop1.read[], khop2.read[], khop3.read[], khop4.read[]];
}

void kep[]
{
khop4.write[0];
}
void kep1Buoc[]
{
khop4.write[khop4.read[] - 1];
delay[50];
}

void moRa[]
{
khop4.write[100];
}
void moRa1Buoc[]
{
khop4.write[khop4.read[] + 1];
delay[50];
}

void moRaTuTu[]
{
for [int i = khop4.read[]; i = 0; i--]
{
khop4.write[i];
delay[10];
}
}
void nangLenTuTu[]
{
for [int i = khop2.read[]; i = 50; i--]
{
khop2.write[i];
delay[50];
}
}
void haXuong1Buoc[]
{
khop2.write[khop2.read[] - 1];
delay[50];
}
void haXuong[]
{
khop2.write[50];
}
void nangLen[]
{
khop2.write[90];
}

void vuonRa[]
{
khop3.write[0];
}

void vuonRa1Buoc[]
{
khop3.write[khop3.read[] - 1];
delay[50];
}
void vuonRaTuTu[]
{
for [int i = khop3.read[]; i >= 0; i--]
{
khop3.write[i];
delay[50];
}
}

void thuVao[]
{
khop3.write[100];
}

void thuVao1Buoc[]
{
khop3.write[khop3.read[] + 1];
delay[50];
}
void thuVaoTuTu[]
{
for [int i = khop3.read[]; i