用joystick控制2自由度舵机运台

下面是测试程序.

 

#include <Servo.h>

#define SERVO1_DPIN 8
#define SERVO2_DPIN 9
#define JOYSTICK_KEY_DPIN 7
#define JOYSTICK_X_APIN A0
#define JOYSTICK_Y_APIN A1


Servo servo1,servo2;
int oriX,oriY;
int minX,maxX,minY,maxY;
bool servo_on;

void setup()
{
  servo_on=true;
  Serial.begin(9600);
  oriX = 0;
  oriY = 0;
  pinMode(JOYSTICK_KEY_DPIN, INPUT);
  servo1.attach(SERVO1_DPIN);
  servo2.attach(SERVO2_DPIN);
}

void loop()
{
    int X,Y;
    float r;
    int key_down = digitalRead(JOYSTICK_KEY_DPIN);
    if(key_down == 0)
    {
      delay(100);
      key_down = digitalRead(JOYSTICK_KEY_DPIN);
      if(key_down==0){
        servo_on=!servo_on;
        Serial.println("Joystick KEY pressed");
      }
    }


   int sensorValue = analogRead(JOYSTICK_X_APIN);
   if(oriX == 0)
   {
     oriX = sensorValue;
     minX=0-oriX;
     maxX=1023-oriX;
     Serial.print("oriX = ");
     Serial.println(oriX);
   }

    X=sensorValue - oriX;
   Serial.print("X = ");
   Serial.println(X);

   if(servo_on){
     if(X>1){
       r=(float)X/(float)maxX;
       servo1.write(90+90*r);
       delay(20);
     }else if(X<-1){
       r=(float)X/(float)minX;
       servo1.write(90-90*r);
       delay(20);
     }else{
       servo1.write(90);
       delay(20);
     }
   }

   sensorValue = analogRead(JOYSTICK_Y_APIN);
   if(oriY == 0)
   {
     oriY = sensorValue;
     minY=0-oriY;
     maxY=1023-oriY;
     Serial.print("oriY = ");
     Serial.println(oriY);
   }

   Y=sensorValue - oriY;
   Serial.print("Y = ");
   Serial.println(Y);

   if(servo_on){
     if(Y>1){
       r=(float)Y/(float)maxY;

       servo2.write(90+90*r);
       delay(20);
     }else if(Y<-1){
       r=(float)Y/(float)minY;
       servo2.write(90-90*r);
       delay(20);
     }else{
       servo2.write(90);
       delay(20);
     }
   }

}

注意几点:

1. 选用的Arduino板子输出电流必须支持舵机的电流要求,如果板子输出电流达不到舵机工作电流,容易损坏板子.我用的是Freaduino,最大支持800ma.

2. joystick的四个方向摇杆是从模拟信号量化到0~1023的数字值, 所以摇杆值转换到舵机的角度信号需要进行换算.

3. 不知道为什么舵机在摇杆角度很小的时候会发生抖动....