#include <ipst.h>
// Define Servo Motor Channel
#define M1 2
#define M2 6
#define M3 10
#define M4 3
#define M5 7
#define M6 11
#define U1 0
#define U2 4
#define U3 8
#define U4 1
#define U5 5
#define U6 9
// Define default position
#define mCCW 850
#define mCW 650
#define uUp 1150
#define uDown 280
int i=0;
int Mx[6]={
2,6,10,3,7,11};
int Ux[6]={
0,4,8,1,5,9};
char x[20];
void setup(){
glcdMode(1);
setTextSize(2);
ZX_SERVO16_Init();
delay(2000);
for(i=0;i<6;i++){
servoWrite(Ux[i],280);
}
for(i=0;i<6;i++){
servoWrite(Mx[i],750);
}
glcd(0,0,"Servo Init");
}
void loop(){
servoWrite(U1,1150);
servoWrite(M1,650);
delay(200);
servoWrite(U1,280);
servoRamp(M1,850);
servoWrite(U4,1150);
servoWrite(M4,850);
delay(200);
servoWrite(U4,280);
servoRamp(M4,650);
servoWrite(U2,1150);
servoWrite(M2,650);
delay(200);
servoWrite(U2,280);
servoRamp(M2,850);
servoWrite(U5,1150);
servoWrite(M5,850);
delay(200);
servoWrite(U5,280);
servoRamp(M5,650);
servoWrite(U3,1150);
servoWrite(M3,650);
delay(200);
servoWrite(U3,280);
servoRamp(M3,850);
servoWrite(U6,1150);
servoWrite(M6,850);
delay(200);
servoWrite(U6,280);
servoRamp(M6,650);
}
void servoRamp(byte CH,int POS){
byte L,H;
Serial1.print("!SC");
Serial1.write(CH);
Serial1.write(0x15);
L=lowByte(POS);
H=highByte(POS);
Serial1.write(L);
Serial1.write(H);
Serial1.write(0x0D);
Serial1.flush();
delay(100);
}
void servoWrite(byte CH,int POS){
byte L,H;
Serial1.print("!SC");
Serial1.write(CH);
Serial1.write(0x00);
L=lowByte(POS);
H=highByte(POS);
Serial1.write(L);
Serial1.write(H);
Serial1.write(0x0D);
Serial1.flush();
delay(100);
}
void ZX_SERVO16_Init(){
out(17,0);
delay(100);
out(17,1);
delay(1000);
Serial1.begin(2400);
Serial1.print("!SCSBR");
Serial1.write(1);
Serial1.write(0x0D);
delay(100);
Serial1.begin(38400);
}