Data hosted with ♥ by Pastebin.com - Download Raw - See Original
  1. #include <ipst.h>
  2. // Define Servo Motor Channel
  3. #define M1  2
  4. #define M2  6
  5. #define M3  10
  6. #define M4  3
  7. #define M5  7
  8. #define M6  11
  9. #define U1  0
  10. #define U2  4
  11. #define U3  8
  12. #define U4  1
  13. #define U5  5
  14. #define U6  9
  15. // Define default position
  16. #define mCCW   850
  17. #define mCW    650
  18. #define uUp    1150
  19. #define uDown  280
  20. int i=0;
  21. int Mx[6]={
  22.   2,6,10,3,7,11};
  23. int Ux[6]={
  24.   0,4,8,1,5,9};
  25. char x[20];
  26.  
  27. void setup(){
  28.   glcdMode(1);
  29.   setTextSize(2);
  30.   ZX_SERVO16_Init();
  31.   delay(2000);
  32.   for(i=0;i<6;i++){
  33.     servoWrite(Ux[i],280);
  34.   }
  35.   for(i=0;i<6;i++){
  36.     servoWrite(Mx[i],750);
  37.   }
  38.   glcd(0,0,"Servo Init");
  39. }
  40. void loop(){
  41.   servoWrite(U1,1150);
  42.   servoWrite(M1,650);
  43.   delay(200);
  44.   servoWrite(U1,280);
  45.   servoRamp(M1,850);
  46.   servoWrite(U4,1150);
  47.   servoWrite(M4,850);
  48.   delay(200);
  49.   servoWrite(U4,280);
  50.   servoRamp(M4,650);
  51.   servoWrite(U2,1150);
  52.   servoWrite(M2,650);
  53.   delay(200);
  54.   servoWrite(U2,280);
  55.   servoRamp(M2,850);
  56.   servoWrite(U5,1150);
  57.   servoWrite(M5,850);
  58.   delay(200);
  59.   servoWrite(U5,280);
  60.   servoRamp(M5,650);
  61.   servoWrite(U3,1150);
  62.   servoWrite(M3,650);
  63.   delay(200);
  64.   servoWrite(U3,280);
  65.   servoRamp(M3,850);
  66.   servoWrite(U6,1150);
  67.   servoWrite(M6,850);
  68.   delay(200);
  69.   servoWrite(U6,280);
  70.   servoRamp(M6,650);
  71. }
  72.  
  73. void servoRamp(byte CH,int POS){
  74.   byte L,H;
  75.   Serial1.print("!SC");
  76.   Serial1.write(CH);
  77.   Serial1.write(0x15);
  78.   L=lowByte(POS);  
  79.   H=highByte(POS);
  80.   Serial1.write(L);
  81.   Serial1.write(H);  
  82.   Serial1.write(0x0D);  
  83.   Serial1.flush();
  84.   delay(100);
  85. }
  86. void servoWrite(byte CH,int POS){
  87.   byte L,H;
  88.   Serial1.print("!SC");
  89.   Serial1.write(CH);
  90.   Serial1.write(0x00);
  91.   L=lowByte(POS);  
  92.   H=highByte(POS);
  93.   Serial1.write(L);
  94.   Serial1.write(H);  
  95.   Serial1.write(0x0D);  
  96.   Serial1.flush();
  97.   delay(100);
  98. }
  99.  
  100. void ZX_SERVO16_Init(){
  101.   out(17,0);
  102.   delay(100);
  103.   out(17,1);
  104.   delay(1000);
  105.   Serial1.begin(2400);
  106.   Serial1.print("!SCSBR");
  107.   Serial1.write(1);
  108.   Serial1.write(0x0D);
  109.   delay(100);
  110.   Serial1.begin(38400);
  111.  
  112. }