timor2542

[4 DOF Robot Arm Keyestudio][Lab 07][ATX-2] 3ServoFirst3Knob and GripperSwitchTest

Aug 2nd, 2021
681
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <ATX2.h>
  2. #define SV4_OPEN_ANGLE 165
  3. #define SV4_CLOSE_ANGLE 85
  4. int x1, x2, x3;
  5. bool SW_L, SW_R;
  6. void setup(){
  7.   OK();
  8.   glcdClear();
  9.  
  10.   glcdMode(1);
  11.   setTextSize(2);
  12.   glcd(1,1,"SV1:");
  13.   glcd(2,1,"SV2:");
  14.   glcd(3,1,"SV3:");
  15.   glcd(4,1,"SV4:");
  16.  
  17.   servo(4, SV4_OPEN_ANGLE);
  18.   setTextColor(GLCD_GREEN);
  19.   glcd(4,5,"OPEN ");
  20. }
  21. void loop(){
  22.  
  23.   x1 = knob(180);
  24.   x2 = map(analog(6), 0, 1023, 30, 90);
  25.   x3 = map(analog(7), 0, 1023, 30, 100);
  26.  
  27.   SW_L = in(28);
  28.   SW_R = in(29);
  29.  
  30.   servo(1, x1);
  31.   servo(2, x2);
  32.   servo(3, x3);
  33.  
  34.   if(SW_L == 0 && SW_R == 1)
  35.   {
  36.     servo(4, SV4_OPEN_ANGLE);
  37.     setTextColor(GLCD_GREEN);
  38.     glcd(4,5,"OPEN ");
  39.    
  40.   }
  41.   else if(SW_L == 1 && SW_R == 0)
  42.   {
  43.     servo(4, SV4_CLOSE_ANGLE);
  44.     setTextColor(GLCD_RED);
  45.     glcd(4,5,"CLOSE");
  46.   }
  47.  
  48.   setTextColor(GLCD_WHITE);
  49.   glcd(1,5," %d   ",x1);
  50.   glcd(2,5," %d   ",x2);
  51.   glcd(3,5," %d   ",x3);
  52. }
  53.  
RAW Paste Data