首页 › Arduino › 菜鸟求助 控制360度舵机的角度

sd8655131 发表于 2015-9-2 20:48:59

菜鸟求助 控制360度舵机的角度

本帖最后由 sd8655131 于 2015-9-2 21:03 编辑

准备搞一个能360度连续旋转的自动云台，本来用步进电机驱动，但发现步进电机不能满足需求。
考虑用舵机，但是问题来了。普通舵机能直接控制角度，但不能连续旋转。而360度的舵机只能通过PWM信号控制旋转方向和速度，不能像普通的舵机那样直接控制角度。

在想能不能通过GY85模块（陀螺仪、加速度计、磁阻罗盘）来作为360度舵机的传感器。当接收到角度信号，通过计算旋转方向以最小的角度旋转到达指定位置。当GY85检测到达应有角度时停止输出PWM信号。

附步进电机版的代码
#include <Wire.h>
#include <EEPROM.h>
#include <Servo.h>
#include "Stepper.h"
#include "SoftModem.h"
SoftModem modem;            //软件猫
//--------------------------------------------------------------------------------------------------
#define DEBUG 1              //是否显示调试信息
#define PI 3.14159265359   //圆周率
#define LedPin 13            //LED管脚
#define LedGps 12            //LED管脚gps ready
#define LedHom 11            //LED管脚home ready
#define SoundPin 8          //蜂鸣器管脚
#define SERVO_V_Pin 10        //俯仰舵机管脚
#define STEPPER_Pins 5,4,3,2 //步进电机管脚
//--------------------------------------------------------------------------------------------------
#define USE_GY85 1           //是否使用电子罗盘(加电后自动调整方位)
#if USE_GY85
  #include "Gy85Support.h"
#endif
//--------------------------------------------------------------------------------------------------
#define UseSoftwareSerial 0  //是否使用软串口读写信息（软串口可避免电脑串口下载程序时与蓝牙冲突）
#if UseSoftwareSerial
  #include "SoftwareSerial.h"
  SoftwareSerial SoftSerial(2,3); // 使用Arduino的D2、D3端口（RX, TX）作为软串口
  #define ExcSerial SoftSerial
#else
  #define ExcSerial Serial
#endif
//--------------------------------------------------------------------------------------------------
void EEPROM_write2(int address, int p) {byte *_pp_=(byte*)&(p);for(int i=0; i<2; i++) EEPROM.write(address+i, _pp_);} //写p到EEPROM
void EEPROM_read2(int address, int& p) {byte *_pp_=(byte*)&(p);for(int i=0; i<2; i++) _pp_=EEPROM.read(address+i);}   //从EEPROM读取p
//--------------------------------------------------------------------------------------------------
int wxs;      //有效卫星数
struct Location {
  double jd;//经度
  double wd;//纬度
  double gd;//高度
};
struct Location HomeLoc = {0,0,0}; //家的坐标
struct Location CurrLoc = {0,0,0}; //当前坐标
struct Location TempLoc = {0,0,0}; //临时坐标
//--------------------------------------------------------------------------------------------------
#define CMD_LEN 16         //命令缓冲区长度
char CmdBuf;      //命令缓冲区
String Cmd;                //命令
bool IsShowLocation=false; //是否显示接收到的坐标数据
//bool IsShowLocation=true;
int AutoSetHome=8;         //自动设置家的卫星数量（0不自动设置家）
int StepsOfCircuit=2046;   //步进电机每转步数
bool HomeReady=false;      //家是否已经设置
int MagAttitude=3;         //电子罗盘姿态( 0:xy 1:yx 2:xz 3:zx 4:yz 5:zy )【正面向上时MagAttitude=0，正面向下时MagAttitude=1 standup is 3】
int QuickOrient=0;         //根据电子罗盘快速定位
int tzjd=0;               
float fjspj,fjgdj;         
//--------------------------------------------------------------------------------------------------
struct Servo_Dgree {
  float H; //水平
  float V; //俯仰
};
struct Servo_Dgree cur_dgree = {0.0, 0.0};
struct Servo_Dgree lst_dgree = {0.0, 0.0};
Stepper stepper;
Servo SERVO_V;                     
int Servo_V_Rev=1;                   //俯仰伺服器是否反向
int SERVO_V_1=1100;                  //俯仰伺服器极限位置1
int SERVO_V_2=1900;                  //俯仰伺服器极限位置2【可通过命令调整该两项，调整俯仰舵机水平和垂直的脉宽】
//--------------------------------------------------------------------------------------------------
void QuickOrientByMag() { //快速定位正北，精度低
  float tlpjd;
  Gy85.GetSensorsData();
  Gy85.CalcMagData();
  switch( MagAttitude ) {
    case 0 : tlpjd=atan2( Gy85.magRaw,Gy85.magRaw); break;
    case 1 : tlpjd=atan2(-Gy85.magRaw,Gy85.magRaw); break;
    case 2 : tlpjd=atan2( Gy85.magRaw,Gy85.magRaw); break;
    case 3 : tlpjd=atan2(-Gy85.magRaw,Gy85.magRaw); break;
    case 4 : tlpjd=atan2( Gy85.magRaw,Gy85.magRaw); break;
    case 5 : tlpjd=atan2(-Gy85.magRaw,Gy85.magRaw); break;
  }
  tlpjd=tlpjd/PI*180;
  stepper.StepNumberYT=0;
  stepper.GotoAngle(tlpjd+tzjd);
  stepper.StepNumberYT=0;
}
//--------------------------------------------------------------------------------------------------
void OrientByMag() { //定位正北，精度高
  int i,j,k=0;
  float tmpv,maxv=-9999.9*9999.9;
  stepper.MoveSteps(1.2*stepper.Phantom+8);
  stepper.StepNumberYT=0;
  for(i=1;i<=stepper.StepsOfCircuit;i++) {
    stepper.MoveSteps(1);
   for(j=0,tmpv=0;j<4;j++) { //取值4次，避免扰动
      Gy85.GetSensorsData();
      switch( MagAttitude ) {
        case 1 :
        case 3 : tmpv+=Gy85.magRaw; break;
        case 0 :
        case 5 : tmpv+=Gy85.magRaw; break;
        case 2 :
        case 4 : tmpv+=Gy85.magRaw; break;
      }
    }
    if( tmpv>maxv ) { maxv=tmpv; k=i; }
  }
  k += stepper.AngleToSteps(tzjd);
  stepper.MoveSteps( stepper.SimplifySteps(k) );
  stepper.StepNumberYT=0;
}
//--------------------------------------------------------------------------------------------------
void setup() {
  ExcSerial.begin(9600); //与手机蓝牙波特率一致
  modem.begin();
  stepper.Init(StepsOfCircuit, STEPPER_Pins); 
  ReadConfig();
  stepper.SetSpeed(8);//表示每分钟转数
  pinMode(LedPin,OUTPUT);
  pinMode(LedGps,OUTPUT);
   pinMode(LedHom,OUTPUT);
  delay(2500); //延时，在此期间手动调整俯仰伺服器使天线指向水平至45度之间，减少伺服器损伤
  SERVO_V.attach(SERVO_V_Pin);
  cur_dgree.H = 0.0;  stepper.StepNumberYT=0;
  lst_dgree.V = 89.0; cur_dgree.V = 90.0;
  Servo_Move(); //调用Servo_Update可能会是水平位置，所以不用
  delay(500);   //等待俯仰伺服器运动到位
#if USE_GY85
  Gy85.Init();
  if( QuickOrient ) QuickOrientByMag(); //根据电子罗盘快速定位正北
  else OrientByMag(); //自检后旋转一周，测出地磁北后，根据安装调整角驱动步进电机使天线指北
#endif
  fjspj=0.0; fjgdj=0.0;
  Servo_Init(); //天线指向水平（正北）
}
//--------------------------------------------------------------------------------------------------
int GPS_UPDATE; //GPS信息是否更新，false：无更新；true：坐标更新
unsigned long CurrTime,GPSUpdateTime=0,SoundTime=0; //当前时间，GPS信息更新时间，上次高音提示收到有效数据时间
int wxsj=-10; //无效数据连续提示次数
//--------------------------------------------------------------------------------------------------
void loop() {
  GPS_UPDATE=ReadGpsInfo();
  CurrTime=millis();
  
  if( GPS_UPDATE>0 ) {//收到有效信息
    digitalWrite(LedGps,HIGH);
    GPSUpdateTime=CurrTime;
    wxsj=-10;
   /// if( CurrTime-SoundTime>1000 ) { //每秒最多提示1次
   ///   SoundTime=CurrTime;
   ///   Spiel(SoundPin,"A",50);       //高音提示
   /// }
  
   // if( GPS_UPDATE ) { //收到有效坐标
      if( IsShowLocation ) ShowLocation(TempLoc);
      if( !HomeReady && AutoSetHome>0 && wxs>=AutoSetHome ) { //自动设置家(卫星数达到自动设置家的值)
        HomeLoc=CurrLoc=TempLoc;
        HomeReady=true;
        digitalWrite(LedHom,HIGH);
        Spiel(SoundPin,"135A",300);   
        delay(400);
        digitalWrite(LedHom,LOW);
        delay(400);
        digitalWrite(LedHom,HIGH);
        delay(400);
        digitalWrite(LedHom,LOW);
        delay(400);
        digitalWrite(LedHom,HIGH);
        delay(400);
        digitalWrite(LedHom,LOW);
        delay(400);
        digitalWrite(LedHom,HIGH);
    }
      if( HomeReady ) {
        if( TempLoc.jd!=CurrLoc.jd || TempLoc.wd!=CurrLoc.wd || TempLoc.gd!=CurrLoc.gd ) {
          CurrLoc=TempLoc;
          CalcAirplaneDgree();
          CalcServoDgree();
        }
     // }
    }

   digitalWrite(LedGps,LOW);
  }
  
 // stepper.MoveToObj(1);
  
  if( CurrTime-GPSUpdateTime>6000 ) { //6秒内未收到有效GPS数据
    GPSUpdateTime=CurrTime-2000;      //每隔2秒报警一次
    digitalWrite(LedPin,HIGH);
    wxsj++;
    Spiel(SoundPin,"a",50);   //低音
  ///digitalWrite(LedGps,LOW);/////
  }
  digitalWrite(LedPin,LOW);


 if( ExcSerial.available()>0) { //检测并执行命令
 
    digitalWrite(LedPin,HIGH);
    int buflen=ExcSerial.readBytes(CmdBuf,CMD_LEN-1);
    ExcSerial.flush();
    CmdBuf=0;
    while( --buflen>=0 ) {
      if( CmdBuf=='\r' || CmdBuf=='\n' )
        CmdBuf=0;
    }      
    if( CmdBuf<'A' || CmdBuf>'Z' ) Cmd=CmdBuf; //部分手机助手会再命令前增加字符'A'-'Z'
    else Cmd=&(CmdBuf);
    if( ExecCmd(Cmd) ) ExcSerial.println("Ok!");
    else               ExcSerial.println("Err!!!");
    digitalWrite(LedPin,LOW);
  }
}
//--------------------------------------------------------------------------------------------------
void AdjustAngle(int angle) { //调整角度，逆时针为正
  int a=stepper.AngleToSteps(angle);
  int c=stepper.StepNumberYT;
  stepper.MoveSteps( stepper.SimplifySteps(a) );
  stepper.StepNumberYT=c;
  tzjd+=angle;
  SaveConfig();
}
//--------------------------------------------------------------------------------------------------
bool ExecCmd(String cmd) {
  ExcSerial.println("Exec Command '"+cmd+"'...");
  if( Cmd=="h" ) { //手动设置家（4颗星以上即可）
    if( wxs<4 ) {
      ExcSerial.println("Satellitic not enough!");
      return false;
    }
    HomeLoc=CurrLoc=TempLoc;
    HomeReady=true;
    ShowLocation(HomeLoc);
        digitalWrite(LedHom,HIGH);
        Spiel(SoundPin,"135A",300);   //成功设置家后演奏音乐
        delay(400);
        digitalWrite(LedHom,LOW);
        delay(400);
        digitalWrite(LedHom,HIGH);
        delay(400);
        digitalWrite(LedHom,LOW);
        delay(400);
        digitalWrite(LedHom,HIGH);
        delay(400);
        digitalWrite(LedHom,LOW);
        delay(400);
        digitalWrite(LedHom,HIGH);
 
  }
  else if( Cmd=="i" ) { //初始化伺服器参数
    SERVO_V_1=1100;
    SERVO_V_2=1900;
    Servo_V_Rev=1;
    MagAttitude=3;
    QuickOrient=0;         //根据电子罗盘快速定位
    tzjd=0; 
    AutoSetHome=8;         //自动设置家的卫星数量（0不自动设置家）
    StepsOfCircuit=2100;
    SaveConfig();
  }
  else if( Cmd=="c" ) { //清空家数据
    HomeReady=false;
    HomeLoc.jd=0;    HomeLoc.wd=0;    HomeLoc.gd=0;
  }
  else if( Cmd=="s" ) { //显示接收到的坐标数据
    ShowLocation(TempLoc);
  }
  else if( Cmd=="sh" ) { //显示家的坐标数据
    ShowLocation(HomeLoc);
  }
  else if( Cmd=="js" ) { //开始监视，显示接收到的坐标数据
    IsShowLocation=true;
  }
  else if( Cmd=="tz" ) { //停止监视
    IsShowLocation=false;
  }
  else if( Cmd=="spfx" ) { //水平伺服器反向
    stepper.RevDir = stepper.RevDir==0 ? 1 : 0 ;
    SaveConfig();
  }
  else if( Cmd=="fyfx" ) { //俯仰伺服器反向
    Servo_V_Rev = Servo_V_Rev==0 ? 1 : 0 ;
    Servo_Move();
    SaveConfig();
  }
  else if( Cmd=="qo" ) { //是否根据电子罗盘快速定位
    QuickOrient = QuickOrient==0 ? 1 : 0 ;
    SaveConfig();
  }
  else if( Cmd=="dw" ) { //重新定位
    if( QuickOrient ) QuickOrientByMag(); //根据电子罗盘快速定位正北
    else OrientByMag(); //自检后旋转一周，测出地磁北后，根据安装调整角驱动步进电机使天线指北
  }
  else if( Cmd=="ma" ) { //电子罗盘姿态变更
    if( ++MagAttitude>=6 ) MagAttitude=0;
    SaveConfig();
  }
  else if( Cmd=="bs+") { StepsOfCircuit+=1;SaveConfig(); } //步进电机每转步数+1
  else if( Cmd=="bs++" ) { StepsOfCircuit+=9;SaveConfig(); } //步进电机每转步数+9
  else if( Cmd=="bs-") { StepsOfCircuit-=1;SaveConfig(); } //步进电机每转步数-1
  else if( Cmd=="bs--" ) { StepsOfCircuit-=9;SaveConfig(); } //步进电机每转步数-9
  else if( Cmd=="xw+") { stepper.Phantom+=1;stepper.TestPhantom(); SaveConfig(); } //步进电机虚位+1
  else if( Cmd=="xw++" ) { stepper.Phantom+=9;stepper.TestPhantom(); SaveConfig(); } //步进电机虚位+9
  else if( Cmd=="xw-") { stepper.Phantom-=1;stepper.TestPhantom(); SaveConfig(); } //步进电机虚位-1
  else if( Cmd=="xw--" ) { stepper.Phantom-=9;stepper.TestPhantom(); SaveConfig(); } //步进电机虚位-9
  else if( Cmd=="jd+") AdjustAngle(1);   //安装调整角度+1
  else if( Cmd=="jd++" ) AdjustAngle(5);   //安装调整角度+5
  else if( Cmd=="jd+++") AdjustAngle(50);//安装调整角度+50
  else if( Cmd=="jd-") AdjustAngle(-1);//安装调整角度-1
  else if( Cmd=="jd--" ) AdjustAngle(-5);//安装调整角度-5
  else if( Cmd=="jd---") AdjustAngle(-50); //安装调整角度-50
  else if( Cmd=="fy1+" ) { SERVO_V_1+=5;SaveConfig(); cur_dgree.V=0;Servo_Update(); } //俯仰伺服器极限位置1脉宽+5
  else if( Cmd=="fy1++") { SERVO_V_1+=50; SaveConfig(); cur_dgree.V=0;Servo_Update(); } //俯仰伺服器极限位置1脉宽+50
  else if( Cmd=="fy1-" ) { SERVO_V_1-=5;SaveConfig(); cur_dgree.V=0;Servo_Update(); } //俯仰伺服器极限位置1脉宽-5
  else if( Cmd=="fy1--") { SERVO_V_1-=50; SaveConfig(); cur_dgree.V=0;Servo_Update(); } //俯仰伺服器极限位置1脉宽-50
  else if( Cmd=="fy2+" ) { SERVO_V_2+=5;SaveConfig(); cur_dgree.V=90; Servo_Update(); } //俯仰伺服器极限位置2脉宽+5
  else if( Cmd=="fy2++") { SERVO_V_2+=50; SaveConfig(); cur_dgree.V=90; Servo_Update(); } //俯仰伺服器极限位置2脉宽+50
  else if( Cmd=="fy2-" ) { SERVO_V_2-=5;SaveConfig(); cur_dgree.V=90; Servo_Update(); } //俯仰伺服器极限位置2脉宽-5
  else if( Cmd=="fy2--") { SERVO_V_2-=50; SaveConfig(); cur_dgree.V=90; Servo_Update(); } //俯仰伺服器极限位置2脉宽-50
  else if( Cmd=="spcs" ) { //测试水平伺服器
    stepper.GotoAngle(-90);
    stepper.GotoAngle(0);
    stepper.GotoAngle(90);
    stepper.GotoAngle(0);
  }
  else if( Cmd=="fycs" ) { //测试俯仰伺服器
    cur_dgree.V = 90;    Servo_Move();
    cur_dgree.V = 0;     Servo_Move();
  }
  else if( Cmd=="cs" ) TestAAT(2000); //测试跟踪
  else if( Cmd=="zdszj" ) { //自动设置家
    AutoSetHome--;
    if( AutoSetHome<0 ) AutoSetHome=8;
    if( AutoSetHome<4 ) AutoSetHome=0;
    SaveConfig();
  }

  else if( Cmd=="?" ) { //显示帮助信息
    ExcSerial.println("h/c;d/s/sh;js/tz;zdszj;cs;q");
    ExcSerial.println(";jd;bs/xw;ma");
  }
#if DEBUG
  else if( Cmd=="d" ) {//显示调试信息
    ExcSerial.println("Servo_H_Rev=" + String(stepper.RevDir));
    ExcSerial.println("Servo_V_Rev=" + String(Servo_V_Rev));
    ExcSerial.println("SERVO_V_1="   + String(SERVO_V_1));
    ExcSerial.println("SERVO_V_2="   + String(SERVO_V_2));
    ExcSerial.println("Auto Home="   + String(AutoSetHome));
    ExcSerial.print("H=");ExcSerial.println(lst_dgree.H);
    ExcSerial.print("V=");ExcSerial.println(lst_dgree.V);
    ExcSerial.println("Adj Angle="   + String(tzjd));
    ExcSerial.println("MagAttitude=" + String(MagAttitude));
    ExcSerial.println("Steps/c="   + String(StepsOfCircuit));
    ExcSerial.println("Phantom="   + String(stepper.Phantom));
    ExcSerial.println("QuickOrient=" + String(QuickOrient));
  }
#endif
  else {
    ExcSerial.println("No such command!");
    return false;
  }
#if DEBUG
  checkFreeMem();
#endif
  return true;
}
//--------------------------------------------------------------------------------------------------
void SaveConfig() {
  EEPROM_write2(10,stepper.RevDir);
  EEPROM_write2(12,Servo_V_Rev);
  EEPROM_write2(14,SERVO_V_1);
  EEPROM_write2(16,SERVO_V_2);
  EEPROM_write2(18,stepper.Phantom);
  EEPROM_write2(20,QuickOrient);
  EEPROM_write2(22,AutoSetHome);
  EEPROM_write2(24,MagAttitude);
  while( tzjd<0   ) tzjd+=360;
  while( tzjd>360 ) tzjd-=360;
  EEPROM_write2(26,tzjd);
  EEPROM_write2(28,StepsOfCircuit);
}
//--------------------------------------------------------------------------------------------------
void ReadConfig() {
  int bd=0x55a3,tl;
  EEPROM_read2(1,tl);
  if( bd!=tl ) { //First Run
    EEPROM_write2(1,bd);
    SaveConfig();
  }
  else {
    EEPROM_read2(10,stepper.RevDir);
    EEPROM_read2(12,Servo_V_Rev);
    EEPROM_read2(14,SERVO_V_1);
    EEPROM_read2(16,SERVO_V_2);
    EEPROM_read2(18,stepper.Phantom);
    EEPROM_read2(20,QuickOrient);
    EEPROM_read2(22,AutoSetHome);
    EEPROM_read2(24,MagAttitude);
    EEPROM_read2(26,tzjd);
    EEPROM_read2(28,StepsOfCircuit);
  }
}
//--------------------------------------------------------------------------------------------------
#if DEBUG
void checkFreeMem() {
  for(int i = 2040;i>0;i--) {
    char* c = (char*)malloc(i);
    if(c) {
      free(c);
      ExcSerial.print("Mem left:");
      ExcSerial.println(i);
      break;
    }
  }
}
#endif
//--------------------------------------------------------------------------------------------------

bh7jec 发表于 2015-9-3 18:44:18

为什么步进无法满足要求？:)

sd8655131 发表于 2015-9-6 08:26:08

bh7jec 发表于 2015-9-3 18:44 static/image/common/back.gif
为什么步进无法满足要求？

速度和转矩达不到要求

bh7jec 发表于 2015-9-7 23:05:31

AAT做的是远距离，并不需要很快的速度。 转矩不够，使用带减速的步进，完全可以达到，而且好用。舵机虽然速度快，但是容易烧。

查看完整版本: 菜鸟求助 控制360度舵机的角度