#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,发球);
}
NOC ----- 机器人参赛车辆应用程序
猜你喜欢
转载自blog.csdn.net/m0_73220913/article/details/131252447
今日推荐
周排行