VEX Robot-2020-Change Up Team 3313B Code
|
2,772
|
0
|
Code,VEXer

1307 字
|
1.1 小时

左边


/*---------------------------------------------------------------------------*/
/*                                                                           */
/*    程序块:  main.cpp                                                       */
/*    发起人:  VEX                                                            */
/*    创  建:  Thu Sep 26 2019                                                */
/*    说  明:  竞赛 模板                                                       */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// LeftBehindMotor      motor         19              
// LeftFrontMotor       motor         17              
// RightFrontMotor      motor         15              
// RightBehindMotor     motor         6               
// LeftLiftMotor        motor         13              
// RightLiftMotor       motor         2               
// LeftSuctionMotor     motor         8               
// RightSuctionMotor    motor         4               
// Controller1          controller                    
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"////////////////头文件
using namespace vex;////////////命名空间

//常数定义
const float Pi=3.1415;///////////////////圆周率常数
const double WheelDiameter=10.5;/////////轮子直径常数
const double WheelSpacing=37;////////////两轮间距常数

//自动时段函数声明
void Amm(int,int=0,int=0,int=40,int=2000);//Auto_Move  声明进退函数
void Arr(int,int=0);///////////////////Auto_Rotate     声明旋转函数
void Axq(int,int=0);////////////Auto_Suction_Ball     声明吸球函数
void Asq(int,int=0);///////////////Auto_Lift_Ball     声明抬升函数

//手动时段函数声明
void Lmm(int Power),Rmm(int Power);///////小车运动函数
void Suction(int Power),Lift(int Power);//////////小球传动函数

// 定义比赛程序的实例
competition Competition;

//自动控制 初始化
void pre_auton(void) { vexcodeInit();}

//自动控制 程序
void autonomous(void) 
{
     //打开架子与旋转 
       Amm(30,20);//..........................打开架子
       Asq(100,-20);//.........................微升小球
     //吸球、转弯、撞框  
       Axq(100);//............................开启吸球
       Lmm(20);Rmm(80);wait(300, msec);//.....转弯
       Amm(80);wait(800, msec);//.............撞框
       Axq(0); Amm(0);//......................停止
     //撞拉调整
       Axq(100);Amm(100);wait(100, msec);//...压入吸球100毫秒
       Amm(-100,10,10,30);//..................初速30、末速100,后退10厘米
       Amm(100);wait(800, msec);//............以100的速度前进800毫秒
       Axq(0); Amm(0);//......................停止
     //小球入框（两个） 
       Asq(100,70);  
     //撞拉吸球
       Axq(100);Amm(100);wait(100, msec);//...压入吸球100毫秒
       Amm(-100,10,10,30);//..................初速30、末速100,后退10厘米
       Amm(100);wait(800, msec);//............以100的速度前进800毫秒
       Axq(0); Amm(0);//......................停止
     //小球入框(第三个)
       Asq(100,70); 
     //退出小车
       Axq(-20);//..........吐球
       Amm(-20,6);//........退出
       Axq(100);//..........吸球
       Amm(-20,14);//.......退出
       Axq(0);//............停止 
}

 //手动控制 程序
void usercontrol(void) {

    int Axis2=Controller1.Axis2.value();////////右摇杆变量
    int Axis3=Controller1.Axis3.value();////////左摇杆变量
    bool CXP=1,CXR=0,CYP=1,CYR=0,CAP=1,CAR=0;///XYAB四按键记忆功能实现变量

  while (1) {

    //按键L1、L2控制吸球电机的转动
    if(Controller1.ButtonL1.pressing()){ CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Suction(100);}
    else if(Controller1.ButtonL2.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Suction(-100);}
    else{if(CXR==0){Suction(0);}}  
   
    //按键R1、R2控制抬升电机的转动  
    if(Controller1.ButtonR1.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Lift(100);}
    else if(Controller1.ButtonR2.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Lift(-100);}
    else{if(CXR==0){Lift(0);}} 

    //按扭XYAB记忆功能的实现
    if(Controller1.ButtonX.pressing()){CYP=1;CYR=0;CAP=1;CAR=0;if(CXP==1){CXR=1;}else{CXR=0;}}else{if(CXR==1){CXP=0;}else{CXP=1;}} 
    if(Controller1.ButtonY.pressing()){CXP=1;CXR=0;CAP=1;CAR=0;if(CYP==1){CYR=1;}else{CYR=0;}}else{if(CYR==1){CYP=0;}else{CYP=1;}}   
    if(Controller1.ButtonA.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;if(CAP==1){CAR=1;}else{CAR=0;}}else{if(CAR==1){CAP=0;}else{CAP=1;}}   
    if(Controller1.ButtonB.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;} 
      
    if(CXR==1){Suction(100);Lift(100);}else if(CXP==0){Suction(0); Lift(0);} 
    if(CYR==1){Suction(100);}else if(CYP==0){Suction(0);} 
    if(CAR==1){Lift(100);}else if(CAP==0){Lift(0);}   
    
    //上下左右箭头键控制小车的前进、后退、顺时针转、逆时针转  
     if(Controller1.ButtonUp.pressing()){Lmm(100);Rmm(100);}//////////////按
     else if(Controller1.ButtonDown.pressing()){Lmm(-100);Rmm(-100);} ////键
     else if(Controller1.ButtonLeft.pressing()){Lmm(-100);Rmm(100);} /////优
     else if(Controller1.ButtonRight.pressing()){Lmm(100);Rmm(-100);}/////先
     else{
        //摇杆控制小车运动  
        Axis2=Controller1.Axis2.value();
        Axis3=Controller1.Axis3.value();
        Lmm(Axis3); 
        Rmm(Axis2); 
     }  
     wait(20, msec); //等待20毫秒
  }
}

//运行主函数开始
int main() {

   //运行自动赛前初始化
   pre_auton();

   //等待场控 运行自动程序
   Competition.autonomous(autonomous);

   //等待场控 运行手动程序
   Competition.drivercontrol(usercontrol);

   //为"防止主函数以无限循环退出"的模块
   while (true) {wait(100, msec);}
    
}
//运行主函数结束

/*
   =========================自动程序函数=========================
   小车前进后退函数：参数  Power    正为前进，负为后退
                   参数  distance 前进或后退距离，为正值
                   参数  sud      速度逐渐增大的距离  
                   参数  inSpeed  加速过程的初速度                                    
                   参数  Time     本过程运行的最大时间，单位为毫秒
*/
void Amm(int Power,int distance,int sud,int inSpeed,int Time)//速度、距离、加速距离、初速度、最大时间
{
      if(Power==0){/////////////////////////////////////////////////////////速度为零时，停止
         LeftFrontMotor.stop();
         LeftBehindMotor.stop();
         RightFrontMotor.stop();
         RightBehindMotor.stop();
     }else if(distance==0){/////////////////////////////////////////////////距离为零时，开启                   
         LeftFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         LeftBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         RightFrontMotor.spin(directionType::rev,Power,velocityUnits::pct);
         RightBehindMotor.spin(directionType::rev,Power,velocityUnits::pct);
     }else{////////////////////////////////////////////////////////////////加速运动程序
         int N=0; 
         int Nmax=abs(Time)/20;////////////////////////////////////////////最大环次数
         int fr=Power/abs(Power);//////////////////////////////////////////正反转变量
         double speed,Angle=0; ////////////////////////////////////////////即进速度变量、通过的路程变量（度数）
         double InitialSpeed=inSpeed; /////////////////////////////////////初速度
         double SpeedUpDistance=abs(sud); /////////////////////////////////加速距离
         double fwdAngle=abs(distance)*360/Pi/WheelDiameter; //////////////前进距离转化为电机转动度数
         double sudAngle=SpeedUpDistance*360/Pi/WheelDiameter; ////////////加速距离转化为电机转动度数
         double InitialAngle=LeftFrontMotor.rotation(rotationUnits::deg);//左前轮马达初始记数的度数
         double nowAngle=InitialAngle; ////////////////////////////////////电机的即时记录数等于初始值
         while(Angle<fwdAngle && N<Nmax)
         {
             N=N+1;
             nowAngle=LeftFrontMotor.rotation(rotationUnits::deg);///////////////////读取马达计数器中的电机转动度数
             Angle=fabs(nowAngle-InitialAngle);//////////////////////////////////////本过程马达转动度数
             if(Angle<sudAngle && sud!=0){///////////////////////////////////////////通过的距离小于加速距离
               speed=fr*((abs(Power)-InitialSpeed)*Angle/sudAngle+InitialSpeed);/////随通过的距离安比例增加速度 
               }
             else{
               speed=Power;//////////////////////////////////////////////////////////以设定的速度运动
               }
             LeftFrontMotor.spin(directionType::fwd,speed,velocityUnits::pct); //////执行
             LeftBehindMotor.spin(directionType::fwd,speed,velocityUnits::pct);//////执行
             RightFrontMotor.spin(directionType::rev,speed,velocityUnits::pct);//////执行
             RightBehindMotor.spin(directionType::rev,speed,velocityUnits::pct);/////执行
             vex::task::sleep(20);
         }
         LeftFrontMotor.stop(brakeType::brake);/////停止、锁定
         LeftBehindMotor.stop(brakeType::brake);////停止、锁定
         RightFrontMotor.stop(brakeType::brake);////停止、锁定
         RightBehindMotor.stop(brakeType::brake);///停止、锁定
     }  
}
///////小车旋转函数  参数：Power 马达速度（正为顺时针、负为逆时针）
////////////////////参数：Angle 小车旋转角度
void Arr(int Power,int Angle)
{
     if(Power==0){/////////////////////停止
         LeftFrontMotor.stop();
         LeftBehindMotor.stop();
         RightFrontMotor.stop();
         RightBehindMotor.stop();
     }else if(Angle==0){///////////////不停旋转
         LeftFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         LeftBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         RightFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         RightBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
     }else{///////////////////////////////旋转一定的角度
         double mPower=Power;
         double mAngle=abs(Angle)*WheelSpacing/WheelDiameter;/////转化为电机的转动角度
         if(mPower<0){mAngle*=-1;mPower*=-1;}
         LeftFrontMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);/////执行
         LeftBehindMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);////执行
         RightFrontMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);////执行  
         RightBehindMotor.rotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);////////执行
         LeftFrontMotor.stop(brakeType::brake);/////////////停止、锁定
         LeftBehindMotor.stop(brakeType::brake);////////////停止、锁定
         RightFrontMotor.stop(brakeType::brake);////////////停止、锁定
         RightBehindMotor.stop(brakeType::brake);///////////停止、锁定   
     }
}  
///////吸球函数      参数：Power  马达速度（正为吸球、负为吐球）
////////////////////参数：Rotate 马达旋转圈数(正为传到指定值，负为开启转到指定值)
void Axq(int Power,int Rotate)
{
    double mPower=Power;
    double mAngle=36*abs(Rotate);
    if(Power==0){///////////////////////////停止转动
        LeftSuctionMotor.stop();
        RightSuctionMotor.stop(); 
    }else if(Rotate==0){////////////////////不停转动
        LeftSuctionMotor.spin(directionType::fwd,Power,velocityUnits::pct);
        RightSuctionMotor.spin(directionType::rev,Power,velocityUnits::pct); 
    }else if(Rotate<0){//////////////////////////////转动一定的圈数（开启）
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftSuctionMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightSuctionMotor.startRotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);          
    }else{//////////////////////////////////传动一定的圈数
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftSuctionMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightSuctionMotor.rotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);  
        LeftSuctionMotor.stop();
        RightSuctionMotor.stop(); 
    }
}
///////抬升函数      参数：Power  马达速度（正为抬球、负为压球）
////////////////////参数：Rotate 马达旋转圈数(正为传到指定值，负为开启转到指定值)
void Asq(int Power,int Rotate)
{
    double mPower=Power;
    double mAngle=36*abs(Rotate);
    if(Power==0){//////////////////////////////////////停止转动
        LeftLiftMotor.stop();
        RightLiftMotor.stop();
    }else if(Rotate==0){//////////////////////////////不停转动
        LeftLiftMotor.spin(directionType::rev,Power,velocityUnits::pct);
        RightLiftMotor.spin(directionType::fwd,Power,velocityUnits::pct);  
    }else if(Rotate<0){//////////////////////////////转动一定的圈数（开启）
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftLiftMotor.startRotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightLiftMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);  
    }else{////////////////////////////////////////////传动一定的圈数（一直）
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftLiftMotor.startRotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightLiftMotor.rotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);  
        LeftLiftMotor.stop();
        RightLiftMotor.stop();
    }
} 
//=========================手动函数=========================
//左前后轮一起转动
void Lmm(int Power)
{
    if(0==Power){/////////////////////////////停止
       LeftBehindMotor.stop();
       LeftFrontMotor.stop();    
    }else{////////////////////////////////////转动
       LeftBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
       LeftFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);   
    }  
}
//右前后轮一起转动
void Rmm(int Power)
{
    if(0==Power){//////////////////////////////////停止
       RightBehindMotor.stop();
       RightFrontMotor.stop();}
    else{//////////////////////////////////////////转动
       RightBehindMotor.spin(directionType::rev,Power,velocityUnits::pct);
       RightFrontMotor.spin(directionType::rev,Power,velocityUnits::pct);}
}
//吸球两臂一起转动
void Suction(int Power)
{
    if(0==Power){/////////////////////////////////停止
       LeftSuctionMotor.stop();
       RightSuctionMotor.stop();
    }else{///////////////////////////////////////转动
       LeftSuctionMotor.spin(directionType::fwd,Power,velocityUnits::pct);
       RightSuctionMotor.spin(directionType::rev,Power,velocityUnits::pct);   
    }    
}
//升球两臂一起转动
void Lift(int Power)
{
    if(0==Power){//////////////////////////////////停止
      LeftLiftMotor.stop();
      RightLiftMotor.stop();
    }else{////////////////////////////////////////转动
      LeftLiftMotor.spin(directionType::rev,Power,velocityUnits::pct);
      RightLiftMotor.spin(directionType::fwd,Power,velocityUnits::pct);    
    }
}

左中


/*---------------------------------------------------------------------------*/
/*                                                                           */
/*    程序块:  main.cpp                                                       */
/*    发起人:  VEX                                                            */
/*    创  建:  Thu Sep 26 2019                                                */
/*    说  明:  竞赛 模板                                                       */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// LeftBehindMotor      motor         19              
// LeftFrontMotor       motor         17              
// RightFrontMotor      motor         15              
// RightBehindMotor     motor         6               
// LeftLiftMotor        motor         13              
// RightLiftMotor       motor         2               
// LeftSuctionMotor     motor         8               
// RightSuctionMotor    motor         4               
// Controller1          controller                    
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"////////////////头文件
using namespace vex;////////////命名空间

//常数定义
const float Pi=3.1415;///////////////////圆周率常数
const double WheelDiameter=10.5;/////////轮子直径常数
const double WheelSpacing=37;////////////两轮间距常数

//自动时段函数声明
void Amm(int,int=0,int=0,int=40,int=2000);//Auto_Move  声明进退函数
void Arr(int,int=0);///////////////////Auto_Rotate     声明旋转函数
void Axq(int,int=0);////////////Auto_Suction_Ball     声明吸球函数
void Asq(int,int=0);///////////////Auto_Lift_Ball     声明抬升函数

//手动时段函数声明
void Lmm(int Power),Rmm(int Power);///////小车运动函数
void Suction(int Power),Lift(int Power);//////////小球传动函数

// 定义比赛程序的实例
competition Competition;

//自动控制 初始化
void pre_auton(void) { vexcodeInit();}

//自动控制 程序
void autonomous(void) 
{
     //打开架子与旋转 
       Amm(30,20);//..........................打开架子
       Asq(100,-20);//........................微升小球
     //吸球、转弯、撞框  
       Axq(100);//............................开启吸球
       Lmm(20);Rmm(80);wait(300, msec);//.....转弯
       Amm(80);wait(800, msec);//.............撞框
       Axq(0); Amm(0);//......................停止
     //撞拉调整
       Axq(100);Amm(100);wait(100, msec);//...压入吸球100毫秒
       Amm(-100,10,10,30);//..................初速30、末速100,后退10厘米
       Amm(100);wait(800, msec);//............以100的速度前进800毫秒
       Axq(0); Amm(0);//......................停止
     //小球入框（两个） 
       Asq(100,70);  
     //撞拉吸球
       Axq(100);Amm(100);wait(100, msec);//...压入吸球100毫秒
       Amm(-100,10,10,30);//..................初速30、末速100,后退10厘米
       Amm(100);wait(800, msec);//............以100的速度前进800毫秒
       Axq(0); Amm(0);//......................停止
     //小球入框(第三个)
       //Asq(100,70); 
     //退出小车
       Axq(-20);//..........吐球
       Amm(-20,6);//........退出
       Axq(0);//..........吸球停止 
       Amm(-30,24);//.......退出
     //小车后退、旋转、前进到中框
       Arr(-65,45);//.........................以速度65逆时针转70xxxxxxxxxxxxxxxx
       Amm(60);wait(800,msec);//..............以速度60前进800毫秒
       Amm(-30,20);//.........................以速度30后退20厘米
       Arr(-65,70);//.........................以速度65逆时针转70xxxxxxxxxxxxxxxx
       Asq(100,-20);//.......................开启升球电机
       Amm(80,120,30,30);//...................以初速30末度80加速30厘米再前进90厘米      
     //小球入中框
       Asq(100,40);
       Amm(-100,20);//.........................以速度100后退8厘米 
}

 //手动控制 程序
void usercontrol(void) {

    int Axis2=Controller1.Axis2.value();////////右摇杆变量
    int Axis3=Controller1.Axis3.value();////////左摇杆变量
    bool CXP=1,CXR=0,CYP=1,CYR=0,CAP=1,CAR=0;///XYAB四按键记忆功能实现变量

  while (1) {

    //按键L1、L2控制吸球电机的转动
    if(Controller1.ButtonL1.pressing()){ CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Suction(100);}
    else if(Controller1.ButtonL2.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Suction(-100);}
    else{if(CXR==0){Suction(0);}}  
   
    //按键R1、R2控制抬升电机的转动  
    if(Controller1.ButtonR1.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Lift(100);}
    else if(Controller1.ButtonR2.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Lift(-100);}
    else{if(CXR==0){Lift(0);}} 

    //按扭XYAB记忆功能的实现
    if(Controller1.ButtonX.pressing()){CYP=1;CYR=0;CAP=1;CAR=0;if(CXP==1){CXR=1;}else{CXR=0;}}else{if(CXR==1){CXP=0;}else{CXP=1;}} 
    if(Controller1.ButtonY.pressing()){CXP=1;CXR=0;CAP=1;CAR=0;if(CYP==1){CYR=1;}else{CYR=0;}}else{if(CYR==1){CYP=0;}else{CYP=1;}}   
    if(Controller1.ButtonA.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;if(CAP==1){CAR=1;}else{CAR=0;}}else{if(CAR==1){CAP=0;}else{CAP=1;}}   
    if(Controller1.ButtonB.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;} 
      
    if(CXR==1){Suction(100);Lift(100);}else if(CXP==0){Suction(0); Lift(0);} 
    if(CYR==1){Suction(100);}else if(CYP==0){Suction(0);} 
    if(CAR==1){Lift(100);}else if(CAP==0){Lift(0);}   
    
    //上下左右箭头键控制小车的前进、后退、顺时针转、逆时针转  
     if(Controller1.ButtonUp.pressing()){Lmm(100);Rmm(100);}//////////////按
     else if(Controller1.ButtonDown.pressing()){Lmm(-100);Rmm(-100);} ////键
     else if(Controller1.ButtonLeft.pressing()){Lmm(-100);Rmm(100);} /////优
     else if(Controller1.ButtonRight.pressing()){Lmm(100);Rmm(-100);}/////先
     else{
        //摇杆控制小车运动  
        Axis2=Controller1.Axis2.value();
        Axis3=Controller1.Axis3.value();
        Lmm(Axis3); 
        Rmm(Axis2); 
     }  
     wait(20, msec); //等待20毫秒
  }
}

//运行主函数开始
int main() {

   //运行自动赛前初始化
   pre_auton();

   //等待场控 运行自动程序
   Competition.autonomous(autonomous);

   //等待场控 运行手动程序
   Competition.drivercontrol(usercontrol);

   //为"防止主函数以无限循环退出"的模块
   while (true) {wait(100, msec);}
    
}
//运行主函数结束

/*
   =========================自动程序函数=========================
   小车前进后退函数：参数  Power    正为前进，负为后退
                   参数  distance 前进或后退距离，为正值
                   参数  sud      速度逐渐增大的距离  
                   参数  inSpeed  加速过程的初速度                                    
                   参数  Time     本过程运行的最大时间，单位为毫秒
*/
void Amm(int Power,int distance,int sud,int inSpeed,int Time)//速度、距离、加速距离、初速度、最大时间
{
      if(Power==0){/////////////////////////////////////////////////////////速度为零时，停止
         LeftFrontMotor.stop();
         LeftBehindMotor.stop();
         RightFrontMotor.stop();
         RightBehindMotor.stop();
     }else if(distance==0){/////////////////////////////////////////////////距离为零时，开启                   
         LeftFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         LeftBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         RightFrontMotor.spin(directionType::rev,Power,velocityUnits::pct);
         RightBehindMotor.spin(directionType::rev,Power,velocityUnits::pct);
     }else{////////////////////////////////////////////////////////////////加速运动程序
         int N=0; 
         int Nmax=abs(Time)/20;////////////////////////////////////////////最大环次数
         int fr=Power/abs(Power);//////////////////////////////////////////正反转变量
         double speed,Angle=0; ////////////////////////////////////////////即进速度变量、通过的路程变量（度数）
         double InitialSpeed=inSpeed; /////////////////////////////////////初速度
         double SpeedUpDistance=abs(sud); /////////////////////////////////加速距离
         double fwdAngle=abs(distance)*360/Pi/WheelDiameter; //////////////前进距离转化为电机转动度数
         double sudAngle=SpeedUpDistance*360/Pi/WheelDiameter; ////////////加速距离转化为电机转动度数
         double InitialAngle=LeftFrontMotor.rotation(rotationUnits::deg);//左前轮马达初始记数的度数
         double nowAngle=InitialAngle; ////////////////////////////////////电机的即时记录数等于初始值
         while(Angle<fwdAngle && N<Nmax)
         {
             N=N+1;
             nowAngle=LeftFrontMotor.rotation(rotationUnits::deg);///////////////////读取马达计数器中的电机转动度数
             Angle=fabs(nowAngle-InitialAngle);//////////////////////////////////////本过程马达转动度数
             if(Angle<sudAngle && sud!=0){///////////////////////////////////////////通过的距离小于加速距离
               speed=fr*((abs(Power)-InitialSpeed)*Angle/sudAngle+InitialSpeed);/////随通过的距离安比例增加速度 
               }
             else{
               speed=Power;//////////////////////////////////////////////////////////以设定的速度运动
               }
             LeftFrontMotor.spin(directionType::fwd,speed,velocityUnits::pct); //////执行
             LeftBehindMotor.spin(directionType::fwd,speed,velocityUnits::pct);//////执行
             RightFrontMotor.spin(directionType::rev,speed,velocityUnits::pct);//////执行
             RightBehindMotor.spin(directionType::rev,speed,velocityUnits::pct);/////执行
             vex::task::sleep(20);
         }
         LeftFrontMotor.stop(brakeType::brake);/////停止、锁定
         LeftBehindMotor.stop(brakeType::brake);////停止、锁定
         RightFrontMotor.stop(brakeType::brake);////停止、锁定
         RightBehindMotor.stop(brakeType::brake);///停止、锁定
     }  
}
///////小车旋转函数  参数：Power 马达速度（正为顺时针、负为逆时针）
////////////////////参数：Angle 小车旋转角度
void Arr(int Power,int Angle)
{
     if(Power==0){/////////////////////停止
         LeftFrontMotor.stop();
         LeftBehindMotor.stop();
         RightFrontMotor.stop();
         RightBehindMotor.stop();
     }else if(Angle==0){///////////////不停旋转
         LeftFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         LeftBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         RightFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         RightBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
     }else{///////////////////////////////旋转一定的角度
         double mPower=Power;
         double mAngle=abs(Angle)*WheelSpacing/WheelDiameter;/////转化为电机的转动角度
         if(mPower<0){mAngle*=-1;mPower*=-1;}
         LeftFrontMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);/////执行
         LeftBehindMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);////执行
         RightFrontMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);////执行  
         RightBehindMotor.rotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);////////执行
         LeftFrontMotor.stop(brakeType::brake);/////////////停止、锁定
         LeftBehindMotor.stop(brakeType::brake);////////////停止、锁定
         RightFrontMotor.stop(brakeType::brake);////////////停止、锁定
         RightBehindMotor.stop(brakeType::brake);///////////停止、锁定   
     }
}  
///////吸球函数      参数：Power  马达速度（正为吸球、负为吐球）
////////////////////参数：Rotate 马达旋转圈数(正为传到指定值，负为开启转到指定值)
void Axq(int Power,int Rotate)
{
    double mPower=Power;
    double mAngle=36*abs(Rotate);
    if(Power==0){///////////////////////////停止转动
        LeftSuctionMotor.stop();
        RightSuctionMotor.stop(); 
    }else if(Rotate==0){////////////////////不停转动
        LeftSuctionMotor.spin(directionType::fwd,Power,velocityUnits::pct);
        RightSuctionMotor.spin(directionType::rev,Power,velocityUnits::pct); 
    }else if(Rotate<0){//////////////////////////////转动一定的圈数（开启）
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftSuctionMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightSuctionMotor.startRotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);          
    }else{//////////////////////////////////传动一定的圈数
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftSuctionMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightSuctionMotor.rotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);  
        LeftSuctionMotor.stop();
        RightSuctionMotor.stop(); 
    }
}
///////抬升函数      参数：Power  马达速度（正为抬球、负为压球）
////////////////////参数：Rotate 马达旋转圈数(正为传到指定值，负为开启转到指定值)
void Asq(int Power,int Rotate)
{
    double mPower=Power;
    double mAngle=36*abs(Rotate);
    if(Power==0){//////////////////////////////////////停止转动
        LeftLiftMotor.stop();
        RightLiftMotor.stop();
    }else if(Rotate==0){//////////////////////////////不停转动
        LeftLiftMotor.spin(directionType::rev,Power,velocityUnits::pct);
        RightLiftMotor.spin(directionType::fwd,Power,velocityUnits::pct);  
    }else if(Rotate<0){//////////////////////////////转动一定的圈数（开启）
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftLiftMotor.startRotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightLiftMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);  
    }else{////////////////////////////////////////////传动一定的圈数（一直）
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftLiftMotor.startRotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightLiftMotor.rotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);  
        LeftLiftMotor.stop();
        RightLiftMotor.stop();
    }
} 
//=========================手动函数=========================
//左前后轮一起转动
void Lmm(int Power)
{
    if(0==Power){/////////////////////////////停止
       LeftBehindMotor.stop();
       LeftFrontMotor.stop();    
    }else{////////////////////////////////////转动
       LeftBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
       LeftFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);   
    }  
}
//右前后轮一起转动
void Rmm(int Power)
{
    if(0==Power){//////////////////////////////////停止
       RightBehindMotor.stop();
       RightFrontMotor.stop();}
    else{//////////////////////////////////////////转动
       RightBehindMotor.spin(directionType::rev,Power,velocityUnits::pct);
       RightFrontMotor.spin(directionType::rev,Power,velocityUnits::pct);}
}
//吸球两臂一起转动
void Suction(int Power)
{
    if(0==Power){/////////////////////////////////停止
       LeftSuctionMotor.stop();
       RightSuctionMotor.stop();
    }else{///////////////////////////////////////转动
       LeftSuctionMotor.spin(directionType::fwd,Power,velocityUnits::pct);
       RightSuctionMotor.spin(directionType::rev,Power,velocityUnits::pct);   
    }    
}
//升球两臂一起转动
void Lift(int Power)
{
    if(0==Power){//////////////////////////////////停止
      LeftLiftMotor.stop();
      RightLiftMotor.stop();
    }else{////////////////////////////////////////转动
      LeftLiftMotor.spin(directionType::rev,Power,velocityUnits::pct);
      RightLiftMotor.spin(directionType::fwd,Power,velocityUnits::pct);    
    }
}

右中


/*---------------------------------------------------------------------------*/
/*                                                                           */
/*    程序块:  main.cpp                                                       */
/*    发起人:  VEX                                                            */
/*    创  建:  Thu Sep 26 2019                                                */
/*    说  明:  竞赛 模板                                                       */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// LeftBehindMotor      motor         19              
// LeftFrontMotor       motor         17              
// RightFrontMotor      motor         15              
// RightBehindMotor     motor         6               
// LeftLiftMotor        motor         13              
// RightLiftMotor       motor         2               
// LeftSuctionMotor     motor         8               
// RightSuctionMotor    motor         4               
// Controller1          controller                    
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"////////////////头文件
using namespace vex;////////////命名空间

//常数定义
const float Pi=3.1415;///////////////////圆周率常数
const double WheelDiameter=10.5;/////////轮子直径常数
const double WheelSpacing=37;////////////两轮间距常数

//自动时段函数声明
void Amm(int,int=0,int=0,int=40,int=2000);//Auto_Move  声明进退函数
void Arr(int,int=0);///////////////////Auto_Rotate     声明旋转函数
void Axq(int,int=0);////////////Auto_Suction_Ball     声明吸球函数
void Asq(int,int=0);///////////////Auto_Lift_Ball     声明抬升函数

//手动时段函数声明
void Lmm(int Power),Rmm(int Power);///////小车运动函数
void Suction(int Power),Lift(int Power);//////////小球传动函数

// 定义比赛程序的实例
competition Competition;

//自动控制 初始化
void pre_auton(void) { vexcodeInit();}

//自动控制 程序
void autonomous(void) 
{
     //打开架子与旋转 
       Amm(30,20);//..........................打开架子
       Asq(100,-20);//........................微升小球
     //吸球、转弯、撞框  
       Axq(100);//............................开启吸球
       Lmm(80);Rmm(20);wait(300, msec);//.....转弯
       Amm(80);wait(800, msec);//.............撞框
       Axq(0); Amm(0);//......................停止
     //撞拉调整
       Axq(100);Amm(100);wait(100, msec);//...压入吸球100毫秒
       Amm(-100,10,10,30);//..................初速30、末速100,后退10厘米
       Amm(100);wait(800, msec);//............以100的速度前进800毫秒
       Axq(0); Amm(0);//......................停止
     //小球入框（两个） 
       Asq(100,70);  
     //撞拉吸球
       Axq(100);Amm(100);wait(100, msec);//...压入吸球100毫秒
       Amm(-100,10,10,30);//..................初速30、末速100,后退10厘米
       Amm(100);wait(800, msec);//............以100的速度前进800毫秒
       Axq(0); Amm(0);//......................停止
     //小球入框(第三个)
       //Asq(100,70); 
     //退出小车
       Axq(-20);//..........吐球
       Amm(-20,6);//........退出
       Axq(0);//..........吸球停止 
       Amm(-30,24);//.......退出
     //小车后退、旋转、前进到中框
       Arr(65,45);//.........................以速度65顺时针转70xxxxxxxxxxxxxxxx
       Amm(60);wait(800,msec);//.............以速度60前进800毫秒
       Amm(-30,20);//........................以速度30后退20厘米
       Arr(65,70);//.........................以速度65顺时针转70xxxxxxxxxxxxxxxx
       Asq(100,-20);//.......................开启升球电机
       Amm(80,120,30,30);//..................以初速30末度80加速30厘米再前进90厘米      
     //小球入中框
       Asq(100,40);
       Amm(-100,20);//........................以速度100后退8厘米 
}

 //手动控制 程序
void usercontrol(void) {

    int Axis2=Controller1.Axis2.value();////////右摇杆变量
    int Axis3=Controller1.Axis3.value();////////左摇杆变量
    bool CXP=1,CXR=0,CYP=1,CYR=0,CAP=1,CAR=0;///XYAB四按键记忆功能实现变量

  while (1) {

    //按键L1、L2控制吸球电机的转动
    if(Controller1.ButtonL1.pressing()){ CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Suction(100);}
    else if(Controller1.ButtonL2.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Suction(-100);}
    else{if(CXR==0){Suction(0);}}  
   
    //按键R1、R2控制抬升电机的转动  
    if(Controller1.ButtonR1.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Lift(100);}
    else if(Controller1.ButtonR2.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Lift(-100);}
    else{if(CXR==0){Lift(0);}} 

    //按扭XYAB记忆功能的实现
    if(Controller1.ButtonX.pressing()){CYP=1;CYR=0;CAP=1;CAR=0;if(CXP==1){CXR=1;}else{CXR=0;}}else{if(CXR==1){CXP=0;}else{CXP=1;}} 
    if(Controller1.ButtonY.pressing()){CXP=1;CXR=0;CAP=1;CAR=0;if(CYP==1){CYR=1;}else{CYR=0;}}else{if(CYR==1){CYP=0;}else{CYP=1;}}   
    if(Controller1.ButtonA.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;if(CAP==1){CAR=1;}else{CAR=0;}}else{if(CAR==1){CAP=0;}else{CAP=1;}}   
    if(Controller1.ButtonB.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;} 
      
    if(CXR==1){Suction(100);Lift(100);}else if(CXP==0){Suction(0); Lift(0);} 
    if(CYR==1){Suction(100);}else if(CYP==0){Suction(0);} 
    if(CAR==1){Lift(100);}else if(CAP==0){Lift(0);}   
    
    //上下左右箭头键控制小车的前进、后退、顺时针转、逆时针转  
     if(Controller1.ButtonUp.pressing()){Lmm(100);Rmm(100);}//////////////按
     else if(Controller1.ButtonDown.pressing()){Lmm(-100);Rmm(-100);} ////键
     else if(Controller1.ButtonLeft.pressing()){Lmm(-100);Rmm(100);} /////优
     else if(Controller1.ButtonRight.pressing()){Lmm(100);Rmm(-100);}/////先
     else{
        //摇杆控制小车运动  
        Axis2=Controller1.Axis2.value();
        Axis3=Controller1.Axis3.value();
        Lmm(Axis3); 
        Rmm(Axis2); 
     }  
     wait(20, msec); //等待20毫秒
  }
}

//运行主函数开始
int main() {

   //运行自动赛前初始化
   pre_auton();

   //等待场控 运行自动程序
   Competition.autonomous(autonomous);

   //等待场控 运行手动程序
   Competition.drivercontrol(usercontrol);

   //为"防止主函数以无限循环退出"的模块
   while (true) {wait(100, msec);}
    
}
//运行主函数结束

/*
   =========================自动程序函数=========================
   小车前进后退函数：参数  Power    正为前进，负为后退
                   参数  distance 前进或后退距离，为正值
                   参数  sud      速度逐渐增大的距离  
                   参数  inSpeed  加速过程的初速度                                    
                   参数  Time     本过程运行的最大时间，单位为毫秒
*/
void Amm(int Power,int distance,int sud,int inSpeed,int Time)//速度、距离、加速距离、初速度、最大时间
{
      if(Power==0){/////////////////////////////////////////////////////////速度为零时，停止
         LeftFrontMotor.stop();
         LeftBehindMotor.stop();
         RightFrontMotor.stop();
         RightBehindMotor.stop();
     }else if(distance==0){/////////////////////////////////////////////////距离为零时，开启                   
         LeftFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         LeftBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         RightFrontMotor.spin(directionType::rev,Power,velocityUnits::pct);
         RightBehindMotor.spin(directionType::rev,Power,velocityUnits::pct);
     }else{////////////////////////////////////////////////////////////////加速运动程序
         int N=0; 
         int Nmax=abs(Time)/20;////////////////////////////////////////////最大环次数
         int fr=Power/abs(Power);//////////////////////////////////////////正反转变量
         double speed,Angle=0; ////////////////////////////////////////////即进速度变量、通过的路程变量（度数）
         double InitialSpeed=inSpeed; /////////////////////////////////////初速度
         double SpeedUpDistance=abs(sud); /////////////////////////////////加速距离
         double fwdAngle=abs(distance)*360/Pi/WheelDiameter; //////////////前进距离转化为电机转动度数
         double sudAngle=SpeedUpDistance*360/Pi/WheelDiameter; ////////////加速距离转化为电机转动度数
         double InitialAngle=LeftFrontMotor.rotation(rotationUnits::deg);//左前轮马达初始记数的度数
         double nowAngle=InitialAngle; ////////////////////////////////////电机的即时记录数等于初始值
         while(Angle<fwdAngle && N<Nmax)
         {
             N=N+1;
             nowAngle=LeftFrontMotor.rotation(rotationUnits::deg);///////////////////读取马达计数器中的电机转动度数
             Angle=fabs(nowAngle-InitialAngle);//////////////////////////////////////本过程马达转动度数
             if(Angle<sudAngle && sud!=0){///////////////////////////////////////////通过的距离小于加速距离
               speed=fr*((abs(Power)-InitialSpeed)*Angle/sudAngle+InitialSpeed);/////随通过的距离安比例增加速度 
               }
             else{
               speed=Power;//////////////////////////////////////////////////////////以设定的速度运动
               }
             LeftFrontMotor.spin(directionType::fwd,speed,velocityUnits::pct); //////执行
             LeftBehindMotor.spin(directionType::fwd,speed,velocityUnits::pct);//////执行
             RightFrontMotor.spin(directionType::rev,speed,velocityUnits::pct);//////执行
             RightBehindMotor.spin(directionType::rev,speed,velocityUnits::pct);/////执行
             vex::task::sleep(20);
         }
         LeftFrontMotor.stop(brakeType::brake);/////停止、锁定
         LeftBehindMotor.stop(brakeType::brake);////停止、锁定
         RightFrontMotor.stop(brakeType::brake);////停止、锁定
         RightBehindMotor.stop(brakeType::brake);///停止、锁定
     }  
}
///////小车旋转函数  参数：Power 马达速度（正为顺时针、负为逆时针）
////////////////////参数：Angle 小车旋转角度
void Arr(int Power,int Angle)
{
     if(Power==0){/////////////////////停止
         LeftFrontMotor.stop();
         LeftBehindMotor.stop();
         RightFrontMotor.stop();
         RightBehindMotor.stop();
     }else if(Angle==0){///////////////不停旋转
         LeftFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         LeftBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         RightFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         RightBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
     }else{///////////////////////////////旋转一定的角度
         double mPower=Power;
         double mAngle=abs(Angle)*WheelSpacing/WheelDiameter;/////转化为电机的转动角度
         if(mPower<0){mAngle*=-1;mPower*=-1;}
         LeftFrontMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);/////执行
         LeftBehindMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);////执行
         RightFrontMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);////执行  
         RightBehindMotor.rotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);////////执行
         LeftFrontMotor.stop(brakeType::brake);/////////////停止、锁定
         LeftBehindMotor.stop(brakeType::brake);////////////停止、锁定
         RightFrontMotor.stop(brakeType::brake);////////////停止、锁定
         RightBehindMotor.stop(brakeType::brake);///////////停止、锁定   
     }
}  
///////吸球函数      参数：Power  马达速度（正为吸球、负为吐球）
////////////////////参数：Rotate 马达旋转圈数(正为传到指定值，负为开启转到指定值)
void Axq(int Power,int Rotate)
{
    double mPower=Power;
    double mAngle=36*abs(Rotate);
    if(Power==0){///////////////////////////停止转动
        LeftSuctionMotor.stop();
        RightSuctionMotor.stop(); 
    }else if(Rotate==0){////////////////////不停转动
        LeftSuctionMotor.spin(directionType::fwd,Power,velocityUnits::pct);
        RightSuctionMotor.spin(directionType::rev,Power,velocityUnits::pct); 
    }else if(Rotate<0){//////////////////////////////转动一定的圈数（开启）
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftSuctionMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightSuctionMotor.startRotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);          
    }else{//////////////////////////////////传动一定的圈数
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftSuctionMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightSuctionMotor.rotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);  
        LeftSuctionMotor.stop();
        RightSuctionMotor.stop(); 
    }
}
///////抬升函数      参数：Power  马达速度（正为抬球、负为压球）
////////////////////参数：Rotate 马达旋转圈数(正为传到指定值，负为开启转到指定值)
void Asq(int Power,int Rotate)
{
    double mPower=Power;
    double mAngle=36*abs(Rotate);
    if(Power==0){//////////////////////////////////////停止转动
        LeftLiftMotor.stop();
        RightLiftMotor.stop();
    }else if(Rotate==0){//////////////////////////////不停转动
        LeftLiftMotor.spin(directionType::rev,Power,velocityUnits::pct);
        RightLiftMotor.spin(directionType::fwd,Power,velocityUnits::pct);  
    }else if(Rotate<0){//////////////////////////////转动一定的圈数（开启）
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftLiftMotor.startRotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightLiftMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);  
    }else{////////////////////////////////////////////传动一定的圈数（一直）
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftLiftMotor.startRotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightLiftMotor.rotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);  
        LeftLiftMotor.stop();
        RightLiftMotor.stop();
    }
} 
//=========================手动函数=========================
//左前后轮一起转动
void Lmm(int Power)
{
    if(0==Power){/////////////////////////////停止
       LeftBehindMotor.stop();
       LeftFrontMotor.stop();    
    }else{////////////////////////////////////转动
       LeftBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
       LeftFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);   
    }  
}
//右前后轮一起转动
void Rmm(int Power)
{
    if(0==Power){//////////////////////////////////停止
       RightBehindMotor.stop();
       RightFrontMotor.stop();}
    else{//////////////////////////////////////////转动
       RightBehindMotor.spin(directionType::rev,Power,velocityUnits::pct);
       RightFrontMotor.spin(directionType::rev,Power,velocityUnits::pct);}
}
//吸球两臂一起转动
void Suction(int Power)
{
    if(0==Power){/////////////////////////////////停止
       LeftSuctionMotor.stop();
       RightSuctionMotor.stop();
    }else{///////////////////////////////////////转动
       LeftSuctionMotor.spin(directionType::fwd,Power,velocityUnits::pct);
       RightSuctionMotor.spin(directionType::rev,Power,velocityUnits::pct);   
    }    
}
//升球两臂一起转动
void Lift(int Power)
{
    if(0==Power){//////////////////////////////////停止
      LeftLiftMotor.stop();
      RightLiftMotor.stop();
    }else{////////////////////////////////////////转动
      LeftLiftMotor.spin(directionType::rev,Power,velocityUnits::pct);
      RightLiftMotor.spin(directionType::fwd,Power,velocityUnits::pct);    
    }
}

右边


/*---------------------------------------------------------------------------*/
/*                                                                           */
/*    程序块:  main.cpp                                                       */
/*    发起人:  VEX                                                            */
/*    创  建:  Thu Sep 26 2019                                                */
/*    说  明:  竞赛 模板                                                       */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// LeftBehindMotor      motor         19              
// LeftFrontMotor       motor         17              
// RightFrontMotor      motor         15              
// RightBehindMotor     motor         6               
// LeftLiftMotor        motor         13              
// RightLiftMotor       motor         2               
// LeftSuctionMotor     motor         8               
// RightSuctionMotor    motor         4               
// Controller1          controller                    
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"////////////////头文件
using namespace vex;////////////命名空间

//常数定义
const float Pi=3.1415;///////////////////圆周率常数
const double WheelDiameter=10.5;/////////轮子直径常数
const double WheelSpacing=37;////////////两轮间距常数

//自动时段函数声明
void Amm(int,int=0,int=0,int=40,int=2000);//Auto_Move  声明进退函数
void Arr(int,int=0);///////////////////Auto_Rotate     声明旋转函数
void Axq(int,int=0);////////////Auto_Suction_Ball     声明吸球函数
void Asq(int,int=0);///////////////Auto_Lift_Ball     声明抬升函数

//手动时段函数声明
void Lmm(int Power),Rmm(int Power);///////小车运动函数
void Suction(int Power),Lift(int Power);//////////小球传动函数

// 定义比赛程序的实例
competition Competition;

//自动控制 初始化
void pre_auton(void) { vexcodeInit();}

//自动控制 程序
void autonomous(void) 
{
     //打开架子与旋转 
       Amm(30,20);//..........................打开架子
       Asq(100,-20);//........................微升小球
     //吸球、转弯、撞框  
       Axq(100);//............................开启吸球
       Lmm(80);Rmm(20);wait(300, msec);//.....转弯
       Amm(80);wait(800, msec);//.............撞框
       Axq(0); Amm(0);//......................停止
     //撞拉调整
       Axq(100);Amm(100);wait(100, msec);//...压入吸球100毫秒
       Amm(-100,10,10,30);//..................初速30、末速100,后退10厘米
       Amm(100);wait(800, msec);//............以100的速度前进800毫秒
       Axq(0); Amm(0);//......................停止
     //小球入框（两个） 
       Asq(100,70);  
     //撞拉吸球
       Axq(100);Amm(100);wait(100, msec);//...压入吸球100毫秒
       Amm(-100,10,10,30);//..................初速30、末速100,后退10厘米
       Amm(100);wait(800, msec);//............以100的速度前进800毫秒
       Axq(0); Amm(0);//......................停止
     //小球入框(第三个)
       Asq(100,70); 
     //退出小车
       Axq(-20);//..........吐球
       Amm(-20,6);//........退出
       Axq(100);//..........吸球
       Amm(-20,14);//.......退出
       Axq(0);//............停止   
}

 //手动控制 程序
void usercontrol(void) {

    int Axis2=Controller1.Axis2.value();////////右摇杆变量
    int Axis3=Controller1.Axis3.value();////////左摇杆变量
    bool CXP=1,CXR=0,CYP=1,CYR=0,CAP=1,CAR=0;///XYAB四按键记忆功能实现变量

  while (1) {

    //按键L1、L2控制吸球电机的转动
    if(Controller1.ButtonL1.pressing()){ CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Suction(100);}
    else if(Controller1.ButtonL2.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Suction(-100);}
    else{if(CXR==0){Suction(0);}}  
   
    //按键R1、R2控制抬升电机的转动  
    if(Controller1.ButtonR1.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Lift(100);}
    else if(Controller1.ButtonR2.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;Lift(-100);}
    else{if(CXR==0){Lift(0);}} 

    //按扭XYAB记忆功能的实现
    if(Controller1.ButtonX.pressing()){CYP=1;CYR=0;CAP=1;CAR=0;if(CXP==1){CXR=1;}else{CXR=0;}}else{if(CXR==1){CXP=0;}else{CXP=1;}} 
    if(Controller1.ButtonY.pressing()){CXP=1;CXR=0;CAP=1;CAR=0;if(CYP==1){CYR=1;}else{CYR=0;}}else{if(CYR==1){CYP=0;}else{CYP=1;}}   
    if(Controller1.ButtonA.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;if(CAP==1){CAR=1;}else{CAR=0;}}else{if(CAR==1){CAP=0;}else{CAP=1;}}   
    if(Controller1.ButtonB.pressing()){CXP=1;CXR=0;CYP=1;CYR=0;CAP=1;CAR=0;} 
      
    if(CXR==1){Suction(100);Lift(100);}else if(CXP==0){Suction(0); Lift(0);} 
    if(CYR==1){Suction(100);}else if(CYP==0){Suction(0);} 
    if(CAR==1){Lift(100);}else if(CAP==0){Lift(0);}   
    
    //上下左右箭头键控制小车的前进、后退、顺时针转、逆时针转  
     if(Controller1.ButtonUp.pressing()){Lmm(100);Rmm(100);}//////////////按
     else if(Controller1.ButtonDown.pressing()){Lmm(-100);Rmm(-100);} ////键
     else if(Controller1.ButtonLeft.pressing()){Lmm(-100);Rmm(100);} /////优
     else if(Controller1.ButtonRight.pressing()){Lmm(100);Rmm(-100);}/////先
     else{
        //摇杆控制小车运动  
        Axis2=Controller1.Axis2.value();
        Axis3=Controller1.Axis3.value();
        Lmm(Axis3); 
        Rmm(Axis2); 
     }  
     wait(20, msec); //等待20毫秒
  }
}

//运行主函数开始
int main() {

   //运行自动赛前初始化
   pre_auton();

   //等待场控 运行自动程序
   Competition.autonomous(autonomous);

   //等待场控 运行手动程序
   Competition.drivercontrol(usercontrol);

   //为"防止主函数以无限循环退出"的模块
   while (true) {wait(100, msec);}
    
}
//运行主函数结束

/*
   =========================自动程序函数=========================
   小车前进后退函数：参数  Power    正为前进，负为后退
                   参数  distance 前进或后退距离，为正值
                   参数  sud      速度逐渐增大的距离  
                   参数  inSpeed  加速过程的初速度                                    
                   参数  Time     本过程运行的最大时间，单位为毫秒
*/
void Amm(int Power,int distance,int sud,int inSpeed,int Time)//速度、距离、加速距离、初速度、最大时间
{
      if(Power==0){/////////////////////////////////////////////////////////速度为零时，停止
         LeftFrontMotor.stop();
         LeftBehindMotor.stop();
         RightFrontMotor.stop();
         RightBehindMotor.stop();
     }else if(distance==0){/////////////////////////////////////////////////距离为零时，开启                   
         LeftFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         LeftBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         RightFrontMotor.spin(directionType::rev,Power,velocityUnits::pct);
         RightBehindMotor.spin(directionType::rev,Power,velocityUnits::pct);
     }else{////////////////////////////////////////////////////////////////加速运动程序
         int N=0; 
         int Nmax=abs(Time)/20;////////////////////////////////////////////最大环次数
         int fr=Power/abs(Power);//////////////////////////////////////////正反转变量
         double speed,Angle=0; ////////////////////////////////////////////即进速度变量、通过的路程变量（度数）
         double InitialSpeed=inSpeed; /////////////////////////////////////初速度
         double SpeedUpDistance=abs(sud); /////////////////////////////////加速距离
         double fwdAngle=abs(distance)*360/Pi/WheelDiameter; //////////////前进距离转化为电机转动度数
         double sudAngle=SpeedUpDistance*360/Pi/WheelDiameter; ////////////加速距离转化为电机转动度数
         double InitialAngle=LeftFrontMotor.rotation(rotationUnits::deg);//左前轮马达初始记数的度数
         double nowAngle=InitialAngle; ////////////////////////////////////电机的即时记录数等于初始值
         while(Angle<fwdAngle && N<Nmax)
         {
             N=N+1;
             nowAngle=LeftFrontMotor.rotation(rotationUnits::deg);///////////////////读取马达计数器中的电机转动度数
             Angle=fabs(nowAngle-InitialAngle);//////////////////////////////////////本过程马达转动度数
             if(Angle<sudAngle && sud!=0){///////////////////////////////////////////通过的距离小于加速距离
               speed=fr*((abs(Power)-InitialSpeed)*Angle/sudAngle+InitialSpeed);/////随通过的距离安比例增加速度 
               }
             else{
               speed=Power;//////////////////////////////////////////////////////////以设定的速度运动
               }
             LeftFrontMotor.spin(directionType::fwd,speed,velocityUnits::pct); //////执行
             LeftBehindMotor.spin(directionType::fwd,speed,velocityUnits::pct);//////执行
             RightFrontMotor.spin(directionType::rev,speed,velocityUnits::pct);//////执行
             RightBehindMotor.spin(directionType::rev,speed,velocityUnits::pct);/////执行
             vex::task::sleep(20);
         }
         LeftFrontMotor.stop(brakeType::brake);/////停止、锁定
         LeftBehindMotor.stop(brakeType::brake);////停止、锁定
         RightFrontMotor.stop(brakeType::brake);////停止、锁定
         RightBehindMotor.stop(brakeType::brake);///停止、锁定
     }  
}
///////小车旋转函数  参数：Power 马达速度（正为顺时针、负为逆时针）
////////////////////参数：Angle 小车旋转角度
void Arr(int Power,int Angle)
{
     if(Power==0){/////////////////////停止
         LeftFrontMotor.stop();
         LeftBehindMotor.stop();
         RightFrontMotor.stop();
         RightBehindMotor.stop();
     }else if(Angle==0){///////////////不停旋转
         LeftFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         LeftBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         RightFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);
         RightBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
     }else{///////////////////////////////旋转一定的角度
         double mPower=Power;
         double mAngle=abs(Angle)*WheelSpacing/WheelDiameter;/////转化为电机的转动角度
         if(mPower<0){mAngle*=-1;mPower*=-1;}
         LeftFrontMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);/////执行
         LeftBehindMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);////执行
         RightFrontMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);////执行  
         RightBehindMotor.rotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);////////执行
         LeftFrontMotor.stop(brakeType::brake);/////////////停止、锁定
         LeftBehindMotor.stop(brakeType::brake);////////////停止、锁定
         RightFrontMotor.stop(brakeType::brake);////////////停止、锁定
         RightBehindMotor.stop(brakeType::brake);///////////停止、锁定   
     }
}  
///////吸球函数      参数：Power  马达速度（正为吸球、负为吐球）
////////////////////参数：Rotate 马达旋转圈数(正为传到指定值，负为开启转到指定值)
void Axq(int Power,int Rotate)
{
    double mPower=Power;
    double mAngle=36*abs(Rotate);
    if(Power==0){///////////////////////////停止转动
        LeftSuctionMotor.stop();
        RightSuctionMotor.stop(); 
    }else if(Rotate==0){////////////////////不停转动
        LeftSuctionMotor.spin(directionType::fwd,Power,velocityUnits::pct);
        RightSuctionMotor.spin(directionType::rev,Power,velocityUnits::pct); 
    }else if(Rotate<0){//////////////////////////////转动一定的圈数（开启）
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftSuctionMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightSuctionMotor.startRotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);          
    }else{//////////////////////////////////传动一定的圈数
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftSuctionMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightSuctionMotor.rotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);  
        LeftSuctionMotor.stop();
        RightSuctionMotor.stop(); 
    }
}
///////抬升函数      参数：Power  马达速度（正为抬球、负为压球）
////////////////////参数：Rotate 马达旋转圈数(正为传到指定值，负为开启转到指定值)
void Asq(int Power,int Rotate)
{
    double mPower=Power;
    double mAngle=36*abs(Rotate);
    if(Power==0){//////////////////////////////////////停止转动
        LeftLiftMotor.stop();
        RightLiftMotor.stop();
    }else if(Rotate==0){//////////////////////////////不停转动
        LeftLiftMotor.spin(directionType::rev,Power,velocityUnits::pct);
        RightLiftMotor.spin(directionType::fwd,Power,velocityUnits::pct);  
    }else if(Rotate<0){//////////////////////////////转动一定的圈数（开启）
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftLiftMotor.startRotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightLiftMotor.startRotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);  
    }else{////////////////////////////////////////////传动一定的圈数（一直）
        if(mPower<0){mAngle*=-1;mPower*=-1;}
        LeftLiftMotor.startRotateFor(-mAngle,rotationUnits::deg,mPower,velocityUnits::pct);
        RightLiftMotor.rotateFor(mAngle,rotationUnits::deg,mPower,velocityUnits::pct);  
        LeftLiftMotor.stop();
        RightLiftMotor.stop();
    }
} 
//=========================手动函数=========================
//左前后轮一起转动
void Lmm(int Power)
{
    if(0==Power){/////////////////////////////停止
       LeftBehindMotor.stop();
       LeftFrontMotor.stop();    
    }else{////////////////////////////////////转动
       LeftBehindMotor.spin(directionType::fwd,Power,velocityUnits::pct);
       LeftFrontMotor.spin(directionType::fwd,Power,velocityUnits::pct);   
    }  
}
//右前后轮一起转动
void Rmm(int Power)
{
    if(0==Power){//////////////////////////////////停止
       RightBehindMotor.stop();
       RightFrontMotor.stop();}
    else{//////////////////////////////////////////转动
       RightBehindMotor.spin(directionType::rev,Power,velocityUnits::pct);
       RightFrontMotor.spin(directionType::rev,Power,velocityUnits::pct);}
}
//吸球两臂一起转动
void Suction(int Power)
{
    if(0==Power){/////////////////////////////////停止
       LeftSuctionMotor.stop();
       RightSuctionMotor.stop();
    }else{///////////////////////////////////////转动
       LeftSuctionMotor.spin(directionType::fwd,Power,velocityUnits::pct);
       RightSuctionMotor.spin(directionType::rev,Power,velocityUnits::pct);   
    }    
}
//升球两臂一起转动
void Lift(int Power)
{
    if(0==Power){//////////////////////////////////停止
      LeftLiftMotor.stop();
      RightLiftMotor.stop();
    }else{////////////////////////////////////////转动
      LeftLiftMotor.spin(directionType::rev,Power,velocityUnits::pct);
      RightLiftMotor.spin(directionType::fwd,Power,velocityUnits::pct);    
    }
}
本文作者 : Sukanu Xian
本文采用 CC BY-NC-SA 4.0 许可协议。转载和引用时请注意遵守协议、注明出处！
本文链接 : https://blog.ssf.moe/238.html
CodeVEX Robot
暂无评论

发送评论 编辑评论 


				

			

						

				

					

						

							

								
							

							
						

					

				

				

					

						

							

								
							

							
						

					

				

				

					

						

							

								
							

							
							
													

					

				

			

			
			

				

					
				

			

				

											

							
							
						

																

							
							
						

																

							
							
						

										
					
											
						

	

		
|´・ω・)ノ
ヾ(≧∇≦*)ゝ
(☆ω☆)
（╯‵□′）╯︵┴─┴
￣﹃￣
(/ω＼)
∠( ᐛ 」∠)＿
(๑•̀ㅁ•́ฅ)
→_→
୧(๑•̀⌄•́๑)૭
٩(ˊᗜˋ*)و
(ノ°ο°)ノ
(´இ皿இ｀)
⌇●﹏●⌇
(ฅ´ω`ฅ)
(╯°A°)╯︵○○○
φ(￣∇￣o)
ヾ(´･ ･｀｡)ノ"
( ง ᵒ̌皿ᵒ̌)ง⁼³₌₃
(ó﹏ò｡)
Σ(っ °Д °;)っ
( ,,´･ω･)ﾉ"(´っω･｀｡)
╮(╯▽╰)╭
o(*////▽////*)q
＞﹏＜
( ๑´•ω•) "(ㆆᴗㆆ)
😂
😀
😅
😊
🙂
🙃
😌
😍
😘
😜
😝
😏
😒
🙄
😳
😡
😔
😫
😱
😭
💩
👻
🙌
🖕
👍
👫
👬
👭
🌚
🌝
🙈
💊
😶
🙏
🍦
🍉
😣
Source: github.com/k4yt3x/flowerhd
	

	

		
颜文字
Emoji
小恐龙
花!
	


									

			

			
			
		

	






上一篇
下一篇 

                    

                    
			        推荐文章
		            

		            

							
WP-China-Yes 一款本地化你的WordPress的插件

							
							

							
记录B站关于视频的api

							
							

							
WRCF 2020 世界机器人大赛总决赛 心得

							
							

							
纯代码在WordPress前端添加网易云全站播放器

							
							

							
Appnode切换php版本时遇到502或者php 404

							
							

							
VEX关键信息传递：关于固件版本马达特性问题

							
							

							
在Typecho更改Gravatar镜像-头像加速

							
							

							
WordPress强迫症优化-去后台Logo 自定义后台底部footer 移除后台登陆标题-WordPress

							
							

							
关于VEX Robot自动时段程序设想

							
							

							
Python 批量图片反转颜色