VEX Robot-2020-Change Up Team 3313B Code

//用于 赛场位置 靠左

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       Team 3313B                                                */
/*    Created:      Sat Oct 03 2020                                           */
/*    Description:  2020·南京 台州市青少年活动中心 VEXRobot V5 代表队 Competition  */
/*                                                                            */
/*----------------------------------------------------------------------------*/

/*
_______   _______      /    _______   _______ 
       |         |    /|           |  |      |
       |         |   / |           |  |      |
_______   _______      |    _______   |______ 
       |         |     |           |  |      |
       |         |     |           |  |      |
_______   _______    _____  _______   |______ 
*/
// ---- START VEXCODE CONFIGURED DEVICES ---- 设备定义开始
// Robot Configuration: 设备定义描述
// [Name] 描述          [Type] 类型    [Port(s)] 端口
// Controller1          controller                    一号遥控器
// LeftLiftMotor        motor         13              左 提升 操作电机
// LeftSuctionMotor     motor         6               左 吸入 操作电机
// LeftFrontMotor       motor         17              左前 移动电机
// LeftBehindMotor      motor         19              左后 移动电机
// RightLiftMotor       motor         8               右 提升 操作电机
// RightSuctionMotor    motor         2               右 吸入 操作电机
// RightFrontMotor      motor         15              右前 移动电机
// RightBehindMotor     motor         4               右后 移动电机
// ---- 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 Am(int,int=0,int=0,int=40,int=2000);//Auto_Move  声明进退函数
void Ar(int,int=0);///////////////////Auto_Rotate     声明旋转函数
void Asb(int,int=0);////////////Auto_Suction_Ball     声明吸球函数
void Alb(int,int=0);///////////////Auto_Lift_Ball     声明抬升函数

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

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

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

//自动控制 程序
void autonomous(void) 
{
     //前进与旋转  
       Am(60,60,30);   //以60的速度前进60厘米
       Ar(-60,155);    //以60的速度逆时针转155度
     //撞框 
       Asb(100);
       Am(100,100,30);  //以100的速度前进100厘米
       wait(100, msec); //等待100毫秒
       Asb(0);
     //小球入框(第一个) 
       Alb(100,7);  

     //回拉小球 
       Asb(100);Am(100);wait(100, msec);//压入吸球100毫秒
       Am(-100,10,10,30);/////////////////初速30、末速100,后退10厘米
       Am(100);wait(800, msec);///////////以100的速度前进800毫秒
       Asb(0); Am(0);
     //小球抬升(第二个) 
       Alb(100,2);

     //回拉小球
       Asb(100);Am(100);wait(100, msec);//压入吸球100毫秒
       Am(-100,10,10,30);/////////////////初速30、末速100,后退10厘米
       Am(100);wait(800, msec);///////////以100的速度前进800毫秒
       Asb(0); Am(0);
     //小球入框(第二个、第三个)
       Alb(100,7); 

     //拉出对方小球
       Asb(100);Am(100);wait(100,msec);//压入吸球100毫秒
       Am(-100,10);//////////////////////退出
       Am(100);wait(800,msec);Am(0);/////撞入
       Asb(-100);//////////吐球
       Am(-20,20);/////////退出
       Asb(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()){L_Motor(50);R_Motor(50);}//////////////按
     else if(Controller1.ButtonDown.pressing()){L_Motor(-50);R_Motor(-50);} ////键
     else if(Controller1.ButtonLeft.pressing()){L_Motor(-80);R_Motor(80);} /////优
     else if(Controller1.ButtonRight.pressing()){L_Motor(80);R_Motor(-80);}/////先
     else{
        //摇杆控制小车运动  
        Axis2=Controller1.Axis2.value();
        Axis3=Controller1.Axis3.value();
        L_Motor(Axis3); 
        R_Motor(Axis2); 
     }  
     wait(20, msec); //等待20毫秒
  }
}

//运行主函数开始
int main() {
   
   //主控显示器内容 BEGIN
    Brain.Screen.clearScreen();
    Brain.Screen.print("_______   _______      /    _______   _______ ");
    Brain.Screen.newLine();
    Brain.Screen.print("       |         |    /|           |  |      |");
    Brain.Screen.newLine(); 
    Brain.Screen.print("       |         |   / |           |  |      |");
    Brain.Screen.newLine(); 
    Brain.Screen.print("_______   _______      |    _______   |______ ");
    Brain.Screen.newLine(); 
    Brain.Screen.print("       |         |     |           |  |      |");
    Brain.Screen.newLine(); 
    Brain.Screen.print("       |         |     |           |  |      |");
    Brain.Screen.newLine(); 
    Brain.Screen.print("_______   _______    _____  _______   |______ ");
    Brain.Screen.newLine(); 
    Brain.Screen.print("Team Members:");
    Brain.Screen.newLine(); 
    Brain.Screen.print("Qiuhan Zang,Lizhen Yang,Hanshao Xian,Jiahao Bao.");
    Brain.Screen.newLine();
    //团队成员 臧秋寒 杨李桢 咸寒少 鲍家豪
  //主控显示器内容 END

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

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

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

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

/*
   =========================自动程序函数=========================
   小车前进后退函数:参数  Power    正为前进,负为后退
                   参数  distance 前进或后退距离,为正值
                   参数  sud      速度逐渐增大的距离  
                   参数  inSpeed  加速过程的初速度                                    
                   参数  Time     本过程运行的最大时间,单位为毫秒
*/
void Am(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 Ar(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 Asb(int Power,int 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{//////////////////////////////////传动一定的圈数
        double mPower=Power;
        double mAngle=360*abs(Rotate);
        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 Alb(int Power,int 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{////////////////////////////////////////////传动一定的圈数
        double mPower=Power;
        double mAngle=360*abs(Rotate);
        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 L_Motor(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 R_Motor(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);    
    }
}
//用于 赛场位置 靠右

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       Team 3313B                                                */
/*    Created:      Sat Oct 03 2020                                           */
/*    Description:  2020·南京 台州市青少年活动中心 VEXRobot V5 代表队 Competition  */
/*                                                                            */
/*----------------------------------------------------------------------------*/

/*
_______   _______      /    _______   _______ 
       |         |    /|           |  |      |
       |         |   / |           |  |      |
_______   _______      |    _______   |______ 
       |         |     |           |  |      |
       |         |     |           |  |      |
_______   _______    _____  _______   |______ 
*/
// ---- START VEXCODE CONFIGURED DEVICES ---- 设备定义开始
// Robot Configuration: 设备定义描述
// [Name] 描述          [Type] 类型    [Port(s)] 端口
// Controller1          controller                    一号遥控器
// LeftLiftMotor        motor         13              左 提升 操作电机
// LeftSuctionMotor     motor         6               左 吸入 操作电机
// LeftFrontMotor       motor         17              左前 移动电机
// LeftBehindMotor      motor         19              左后 移动电机
// RightLiftMotor       motor         8               右 提升 操作电机
// RightSuctionMotor    motor         2               右 吸入 操作电机
// RightFrontMotor      motor         15              右前 移动电机
// RightBehindMotor     motor         4               右后 移动电机
// ---- 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 Am(int,int=0,int=0,int=40,int=2000);//Auto_Move  声明进退函数
void Ar(int,int=0);///////////////////Auto_Rotate     声明旋转函数
void Asb(int,int=0);////////////Auto_Suction_Ball     声明吸球函数
void Alb(int,int=0);///////////////Auto_Lift_Ball     声明抬升函数

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

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

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

//自动控制 程序
void autonomous(void) 
{
     //前进与旋转  
       Am(60,60,30);   //以60的速度前进60厘米
       Ar(60,155);    //以60的速度顺时针转155度
     //撞框 
       Asb(100);
       Am(100,100,30);  //以100的速度前进100厘米
       wait(100, msec); //等待100毫秒
       Asb(0);
     //小球入框(第一个) 
       Alb(100,7);  

     //回拉小球 
       Asb(100);Am(100);wait(100, msec);//压入吸球100毫秒
       Am(-100,10,10,30);/////////////////初速30、末速100,后退10厘米
       Am(100);wait(800, msec);///////////以100的速度前进800毫秒
       Asb(0); Am(0);
     //小球抬升(第二个) 
       Alb(100,2);

     //回拉小球
       Asb(100);Am(100);wait(100, msec);//压入吸球100毫秒
       Am(-100,10,10,30);/////////////////初速30、末速100,后退10厘米
       Am(100);wait(800, msec);///////////以100的速度前进800毫秒
       Asb(0); Am(0);
     //小球入框(第二个、第三个)
       Alb(100,7); 

     //拉出对方小球
       Asb(100);Am(100);wait(100,msec);//压入吸球100毫秒
       Am(-100,10);//////////////////////退出
       Am(100);wait(800,msec);Am(0);/////撞入
       Asb(-100);//////////吐球
       Am(-20,20);/////////退出
       Asb(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()){L_Motor(50);R_Motor(50);}//////////////按
     else if(Controller1.ButtonDown.pressing()){L_Motor(-50);R_Motor(-50);} ////键
     else if(Controller1.ButtonLeft.pressing()){L_Motor(-80);R_Motor(80);} /////优
     else if(Controller1.ButtonRight.pressing()){L_Motor(80);R_Motor(-80);}/////先
     else{
        //摇杆控制小车运动  
        Axis2=Controller1.Axis2.value();
        Axis3=Controller1.Axis3.value();
        L_Motor(Axis3); 
        R_Motor(Axis2); 
     }  
     wait(20, msec); //等待20毫秒
  }
}

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

    //主控显示器内容 BEGIN
    Brain.Screen.clearScreen();
    Brain.Screen.print("_______   _______      /    _______   _______ ");
    Brain.Screen.newLine();
    Brain.Screen.print("       |         |    /|           |  |      |");
    Brain.Screen.newLine(); 
    Brain.Screen.print("       |         |   / |           |  |      |");
    Brain.Screen.newLine(); 
    Brain.Screen.print("_______   _______      |    _______   |______ ");
    Brain.Screen.newLine(); 
    Brain.Screen.print("       |         |     |           |  |      |");
    Brain.Screen.newLine(); 
    Brain.Screen.print("       |         |     |           |  |      |");
    Brain.Screen.newLine(); 
    Brain.Screen.print("_______   _______    _____  _______   |______ ");
    Brain.Screen.newLine(); 
    Brain.Screen.print("Team Members:");
    Brain.Screen.newLine(); 
    Brain.Screen.print("Qiuhan Zang,Lizhen Yang,Hanshao Xian,Jiahao Bao.");
    Brain.Screen.newLine();
    //团队成员 臧秋寒 杨李桢 咸寒少 鲍家豪
    //主控显示器内容 END

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

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

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

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

/*
   =========================自动程序函数=========================
   小车前进后退函数:参数  Power    正为前进,负为后退
                   参数  distance 前进或后退距离,为正值
                   参数  sud      速度逐渐增大的距离  
                   参数  inSpeed  加速过程的初速度                                    
                   参数  Time     本过程运行的最大时间,单位为毫秒
*/
void Am(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 Ar(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 Asb(int Power,int 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{//////////////////////////////////传动一定的圈数
        double mPower=Power;
        double mAngle=360*abs(Rotate);
        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 Alb(int Power,int 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{////////////////////////////////////////////传动一定的圈数
        double mPower=Power;
        double mAngle=360*abs(Rotate);
        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 L_Motor(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 R_Motor(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);    
    }
}
本文作者 : 呆憨 小咸鱼
本文采用 CC BY-NC-SA 4.0 许可协议。转载和引用时请注意遵守协议、注明出处!
本文链接 : https://blog.daihan.top/vex-robot2020change-up-team-3313b-code.html
暂无评论

发送评论 编辑评论


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