NOC ----- 机器人参赛车辆应用程序

#include <motor.h>
#include <pskey.h>

volatile int 吸球状态1;
volatile int 吸球状态2;
volatile int 发球状态1;
volatile int 发球状态2;
Motor motor;
Ps2Handle pskey;
static bool runOnce3r8fn3=true;

void 向前走(int 速度, int 时间) {
  motor.motor_set_pwm(1,速度);
  motor.motor_set_pwm(2,速度);
  motor.motor_set_pwm(3,(0 - 速度));
  motor.motor_set_pwm(4,(0 - 速度));
  delay(时间);
  motor.motor_set_pwm(1,0);
  motor.motor_set_pwm(2,0);
  motor.motor_set_pwm(3,0);
  motor.motor_set_pwm(4,0);
}

void 向后走(int 速度, int 时间) {
  motor.motor_set_pwm(3,速度);
  motor.motor_set_pwm(4,速度);
  motor.motor_set_pwm(1,(0 - 速度));
  motor.motor_set_pwm(2,(0 - 速度));
  delay(时间);
  motor.motor_set_pwm(1,0);
  motor.motor_set_pwm(2,0);
  motor.motor_set_pwm(3,0);
  motor.motor_set_pwm(4,0);
}

void 向左转(int 速度, int 时间) {
  motor.motor_set_pwm(3,(0 - 速度));
  motor.motor_set_pwm(4,(0 - 速度));
  delay(时间);
  motor.motor_set_pwm(3,0);
  motor.motor_set_pwm(4,0);
}

void 向右转(int 速度, int 时间) {
  motor.motor_set_pwm(1,速度);
  motor.motor_set_pwm(2,速度);
  delay(时间);
  motor.motor_set_pwm(1,0);
  motor.motor_set_pwm(2,0);
}

void 吸球2(int 吸球, int 时间) {
  motor.motor_set_pwm(5,(0 - 吸球));
  motor.motor_set_pwm(6,(0 - 吸球));
  delay(时间);
  motor.motor_set_pwm(5,0);
  motor.motor_set_pwm(6,0);
}

void 发球2(int 电梯, int 发球, int 吸球, int 时间) {
  motor.motor_set_pwm(8,(0 - 发球));
  motor.motor_set_pwm(9,发球);
  motor.motor_set_pwm(10,(0 - 发球));
  motor.motor_set_pwm(5,(0 - 吸球));
  motor.motor_set_pwm(6,(0 - 吸球));
  delay(1000);
  motor.motor_set_pwm(7,(0 - 电梯));
  delay(时间);
  motor.motor_set_pwm(8,0);
  motor.motor_set_pwm(9,0);
  motor.motor_set_pwm(10,0);
  motor.motor_set_pwm(7,0);
  motor.motor_set_pwm(5,0);
  motor.motor_set_pwm(6,0);
}

void setup(){
  吸球状态1 = 0;
  吸球状态2 = 0;
  发球状态1 = 0;
  发球状态2 = 0;
  motor.motor_init();
  motor.motor_revert(32);
  motor.motor_set_mode(6,2);
  pskey.ps2_init();
}

void loop(){
  pskey.ps2_dokey(8);
  int 左 = 0;
  int 右 = 0;
  int 吸球 = 0;
  int 发球 = 0;
  int 电梯 = 0;
  //run once only.;
  while (runOnce3r8fn3 && pskey.ps2_is_key_pressed(PS2_KEY_START_IDX,0)) {
    runOnce3r8fn3=false;

    向前走(127, 1000);
    向前走(127, 500);
    吸球2(127, 1000);
    向后走(50, 200);
    向左转(50, 500);
    向前走(127, 200);
    吸球2(127, 1000);
    向右转(50, 1000);
    向前走(127, 200);
    吸球2(127, 1000);
    发球2(127, 127, 127, 3000);
  }
  左 = pskey.ps2_get_ly();
  右 = pskey.ps2_get_ry();
  if (pskey.ps2_is_key_pressed(PS2_KEY_R1_IDX,0)) {
    if (吸球状态1 == 1) {
      if (吸球状态2 == 1) {
        吸球状态2 = 0;

      } else {
        吸球状态2 = 1;

      }
      吸球状态1 = 0;

    }

  } else {
    吸球状态1 = 1;

  }
  if (吸球状态2 == 1) {
    吸球 = -127;

  } else {
    吸球 = 0;

  }
  if (pskey.ps2_is_key_pressed(PS2_KEY_L1_IDX,0)) {
    if (发球状态1 == 1) {
      if (发球状态2 == 1) {
        发球状态2 = 0;

      } else {
        发球状态2 = 1;

      }
      发球状态1 = 0;

    }

  } else {
    发球状态1 = 1;

  }
  if (发球状态2 == 1) {
    发球 = -127;

  } else {
    发球 = 0;

  }
  if (pskey.ps2_is_key_pressed(PS2_KEY_R2_IDX,0)) {
    电梯 = -127;

  } else if (pskey.ps2_is_key_pressed(PS2_KEY_CROSS_IDX,0)) {
    电梯 = 127;
    吸球 = 127;
  } else {
    电梯 = 0;

  }
  motor.motor_set_pwm(1,左);
  motor.motor_set_pwm(2,左);
  motor.motor_set_pwm(3,(0 - 右));
  motor.motor_set_pwm(4,(0 - 右));
  motor.motor_set_pwm(5,吸球);
  motor.motor_set_pwm(6,吸球);
  motor.motor_set_pwm(7,电梯);
  motor.motor_set_pwm(8,发球);
  motor.motor_set_pwm(9,(0 - 发球));
  motor.motor_set_pwm(10,发球);

}