全向电机控制

只看该作者 · 2010-9-1 15:55

#include <GLOBAL.h>
#include <stdlib.h>
#include <stdbool.h>
#include <msp430x12x.h>
//------------------------------------------------------------------------ValveConTime=200 µç»ú¿ªÊ§¿ØÊ±¼ä
#define MotoConOneOut          P3DIR |=  BIT6
#define MotoConOneHig          P3OUT |=  BIT6
#define MotoConOneLow          P3OUT &=~ BIT6
#define MotoConTwoOut          P3DIR |=  BIT7
#define MotoConTwoHig          P3OUT |=  BIT7
#define MotoConTwoLow          P3OUT &=~ BIT7
//------·§ÃÅ×´Ì¬¼ì²âµçÔ´¹©¸ø
#define ValveStChkPowerDirOut    P3DIR |=  BIT0
#define ValveStChkPowerHig       P3OUT |=  BIT0
#define ValveStChkPowerLow       P3OUT &=~ BIT0
//------·§ÃÅ×´Ì¬
#define ValveStOneIn             P3DIR &=~ BIT1
#define ValveStTwoIn             P3DIR &=~ BIT2
#define ValveStOne             P3IN  & BIT1
#define ValveStTwo             P3IN  & BIT2
void MotoInit(void)
{
MotoConOneOut;
MotoConTwoOut;
MotoConOneLow;
MotoConTwoLow;
MotoConOver=1;
ValveReverse=0;
ValveStChkPowerDirOut;
ValveStChkPowerLow;
}
void MotoOpen(void)
{
ValveStChkPowerHig;
if(ValveConTimes<=185)//---ÂýËÙÔËÐÐ
{
      MotoConOneLow;
      MotoConTwoHig;
      WDT();
      Delay(1000);//---ÔËÐÐ
      MotoConOneLow;
      MotoConTwoLow;
      WDT();
      Delay(1000);//---Í£Ö¹
}
else if(ValveConTimes<=190)//---ÖÐËÙÔËÐÐ
{
      MotoConOneLow;
      MotoConTwoHig;
      WDT();
      Delay(2000);//---ÔËÐÐ
      MotoConOneLow;
      MotoConTwoLow;
      WDT();
      Delay(1000);//---Í£Ö¹
}
else if(ValveConTimes<=195)//---¸ßËÙÔËÐÐ
{
      MotoConOneLow;
      MotoConTwoHig;
}
else if(ValveConTimes<=196)//---Í£Ö¹
{
      MotoConOneLow;
      MotoConTwoLow;
}
else if(ValveConTimes<=198)//---·´ÏòÔËÐÐ
{
      MotoConOneHig;
      MotoConTwoLow;
}
else
{
      MotoConOneLow;
      MotoConTwoLow;
}
}
void MotoClos(void)
{
ValveStChkPowerHig;
if(ValveConTimes<=185)//---ÂýËÙÔËÐÐ
{
      MotoConOneHig;
      MotoConTwoLow;
      WDT();
      Delay(1000);//---ÔËÐÐ
      MotoConOneLow;
      MotoConTwoLow;
      WDT();
      Delay(1000);//---Í£Ö¹
}
else if(ValveConTimes<=190)//---ÖÐËÙÔËÐÐ
{
      MotoConOneHig;
      MotoConTwoLow;
      WDT();
      Delay(2000);//---ÔËÐÐ
      MotoConOneLow;
      MotoConTwoLow;
      WDT();
      Delay(1000);//---Í£Ö¹
}
else if(ValveConTimes<=195)//---¸ßËÙÔËÐÐ
{
      MotoConOneHig;
      MotoConTwoLow;
}
else if(ValveConTimes<=196)//---Í£Ö¹
{
      MotoConOneLow;
      MotoConTwoLow;
}
else if(ValveConTimes<=198)//---·´ÏòÔËÐÐ
{
      MotoConOneLow;
      MotoConTwoHig;
}
else
{
      MotoConOneLow;
      MotoConTwoLow;
}
}
void ValveStChk(void)
{
ValveOpen=0;//
ValveClos=0;//---·§ÃÅ¹Ø
ValveErrSt=1;//------------·§ÃÅ´íÎó
if(ValveStOne&BIT1)
{
      if(!(ValveStTwo&BIT2))
      {
         MeterSts[1]&=0xfc;//--------Çå³ýµÍÁ½Î»---¹Ø·§×´Ì¬
         ValveClos=1;//---·§ÃÅ¹Ø
         ValveErrSt=0;
      }
      else
      {
         MeterSts[1]|=0x02;//--------Ë«¸ß---·§ÃÅ´íÎó
      }
}
else
{
      if(ValveStTwo&BIT2)
      {
         MeterSts[1]&=0xfc;//Çå³ýµÍÁ½Î»---¹Ø·§×´Ì¬
         MeterSts[1]|=0x01;//--------¿ª×´Ì¬
         ValveOpen=1;//-------------·§ÃÅ¿ª
         ValveErrSt=0;
      }
}
}
void MotoStop(void)
{
SleepEn=1;
MotoConEn=0;
MotoClosEn=0;
MotoOpenEn=0;
ReverseClos=0;
ReverseOpen=0;
MotoConOneOut;
MotoConTwoOut;
MotoConOneLow;
MotoConTwoLow;
ValveReverse=0;
ValveConTimes=0;
ReverseConData=0;
ValveStChkPowerDirOut;
ValveStChkPowerLow;
MotoConOver=1;
}
void ValvePower(void)
{
ValveStChkPowerHig;
ValveStOneIn;
ValveStTwoIn;
MotoConOneOut;
MotoConTwoOut;
MotoConOneLow;
MotoConTwoLow;
WDT();
Delay(1000);
SleepEn=0;
MotoConEn=1;
ValveConTimes=200;
}
//---void MotoStr(bool MotoSt)--------------------------------------------------
//---¹¦ÄÜÃèÊö:-
//---ÊäÈëÌõ¼þ:-MotoSt=1,Ö´ÐÐ¿ª·§¶¯×÷·ñÔòÖ´ÐÐ¹Ø·§¶¯×÷¡£
//---Êä³öÏîÄ¿:-ÎÞ¡£
void MotoStr(bool MotoSt)
{
ValvePower();
if(MotoSt)
{
      MotoOpenEn=1;
      MotoClosEn=0;
}
else
{
      MotoOpenEn=0;
      MotoClosEn=1;
}
}
void ValveNormalCon(void)
{
ValveStChk();
WDT();
SleepEn=0;
if(MotoOpenEn)//------------------------------------------¿ª·§¶¯×÷
{
      if(ValveLockSt|PowerLowFg)//----Èç¹û¶ÏÏß£¬µÍÑ¹£¬Í¸Ö§£¬½ûÖ¹¿ª·§
      {
         MotoOpenEn=0;
         MotoClosEn=1;
      }
      else
      {
         if(ValveClos|ValveErrSt)
         {
            MotoOpen();
         }
         else
         {
            if(ValveOpen)
            {
                  MotoStop();
            }
            else
            {
                  MotoOpen();
            }
         }
      }
}
else
{
      if(MotoClosEn)//--------------------------------------¹Ø·§¶¯×÷
      {
         if(ValveOpen|ValveErrSt)
         {
            MotoClos();
         }
         else
         {
            if(ValveClos)
            {
                  MotoStop();
            }
            else
            {
                  MotoClos();
            }
         }
      }
}
}
void ValveReverseCon(void)
{
ValveStChk();
if(ReverseOpen)
{
      if(ValveConTimes>198)//---·´ÏòÔËÐÐ
      {
         MotoConOneHig;
         MotoConTwoLow;
      }
      else
      {
         if(ValveOpen)
         {
            MotoStop();
         }
      }
}
else
{
      MotoStop();
}
}
void ValveCon(void)
{
if(MotoConEn)
{
      if(ValveConTimes!=0)
      {
         if(ValveReverse)
         {
            ValveReverseCon();
         }
         else
         {
            ValveNormalCon();
         }
      }
      else
      {
         MotoStop();
      }
}
}
//---看似简单的问题，其实已经修改了N次，经过最后的琢磨定型了以上的控制方式---//