task main()
{
while(true)
{
getJoystickSettings(joystick);
if (joy1Btn(5) == 1)
single_main_wheel_fast(joystick.joy1_y2, joystick.joy1_x2);
else
single_main_wheel(joystick.joy1_y2, joystick.joy1_x2);
lift(joystick.joy2_y1);
if (joy2Btn(6) == 1)
winchmove(joystick.joy2_y2);
else motor[winch] = 0;
servo_in(joy2Btn(4));
servo_out(joy2Btn(2));
servo_stop(joy2Btn(3));
/*
servoFlat(joy2Btn(1));
servoDrop(joy2Btn(2));
servoRaise(joy2Btn(4));
servoAngleDown(joy2Btn(3));
*/
flagmove(joystick.joy1_x1);
wait1Msec(TIME_INTERVAL);
}
}
int servo_start(void) // Take cog & start servo process
{
lockID = locknew(); // Check out a lock
if (lockID == -1) return -1; // Return -1 if no locks
servo_stop(); // Stop in case cog is running
servoCog = cogstart(&servo, NULL, stack, // Launch servo into new cog
sizeof(stack)) + 1;
return servoCog; // Return cog that was taken
}
int main() // main function
{
servo_angle(16, 0); // P16 servo to 0 degrees
pause(500); // Allow 1/2 second to get there
servo_setramp(16, 7); // Change by up to 7/10 degree/20 ms
servo_angle(16, 1800); // P16 servo to 180 degrees
pause(6000); // Allow 6 seconds to get there
// (with ramping in effect)
servo_stop(); // Stop servo process
}
void car_movement_control(struct movement_info *cmd)
{
if (cmd->car_forward == 0x01) //前进控制
{
DC_Motor(1, 0, 60); //右轮正转
//Delay_ms(30);
DC_Motor(2, 0, 50); //左轮正转
}
else if (cmd->car_back == 0x01) //后退控制
{
DC_Motor(1, 2, 70); //右轮反转
Delay_ms(10);
DC_Motor(2, 2, 90); //左轮反转
}
else if (cmd->car_right == 0x01) //右转控制
{
DC_Motor(1, 2, 40); //右轮反转
DC_Motor(2, 0, 50); //左轮大速度（此处电机有问题给很大的占空比，转速仍很小）
}
else if (cmd->car_left == 0x01)
{
DC_Motor(1, 0, 20); //右轮大速度
DC_Motor(2, 2, 20); //左轮反转
}
else if (cmd->car_stop == 0x01)
{
DC_Motor(1, 1, 0); //右轮停止
DC_Motor(2, 1, 0); //左轮停止
}
else
{}
if (cmd->steer_left == 0x01) //舵机左转
{
servo_turn_left();
}
else if (cmd->steer_right == 0x01) //舵机右转
{
servo_turn_right();
}
else if (cmd->steer_stop == 0x01) //舵机停止
{
servo_stop();
}
else
{}
}
