void AGV_rotate(void)
{
int tmp = 0;
AGV_V_set(ACON_ROTATION_SPEED);
// motor_speed_set(ROTATION_MOTOR,ACON_ROTATION_SPEED);
switch(AGV_status.AGV_control_p->data.rotating_data.rotating_towards)
{
case LEFT:
motor_run(LEFT_MOTOR,CCW);
motor_run(RIGHT_MOTOR,CCW);
//motor_run(ROTATION_MOTOR,CCW);
tmp = AGV_status.runing_towards - AGV_status.AGV_control_p->data.rotating_data.rotating_degree;
break;
case RIGHT:
motor_run(LEFT_MOTOR,CW);
motor_run(RIGHT_MOTOR,CW);
//motor_run(ROTATION_MOTOR,CW);
tmp = AGV_status.runing_towards + AGV_status.AGV_control_p->data.rotating_data.rotating_degree;
break;
}
if(tmp >= 360)
{
tmp -= 360;
}
else if(tmp < 0)
{
tmp += 360;
}
AGV_status.updata_waitting_status = DRGREE_UPDATA_WRITING; //由于陀螺仪默认状态下一直在更新角度状态，因此此赋值暂无意义
AGV_status.rotating_status = 1;
AGV_status.runing_status = 0;
AGV_status.runing_towards = tmp;
}
void AGV_run(void)
{
AGV_V_set( ACON_RUN_SPEED_INIT);
//AGV_status.V_Set = ACON_RUN_SPEED_INIT;
motor_run(LEFT_MOTOR,CW);
motor_run(RIGHT_MOTOR,CCW);
AGV_status.runing_status = 1;
AGV_status.rotating_status = 0;
}
void test(step_t m){
std::cout << "move motor to " << m << '\n';
motor_run(m);
while(true == run_flg){
isr_motor_step();
std::cout << motor_pos << ' ' << c32 << ' ' << c << '\n';
std::this_thread::sleep_for(std::chrono::microseconds(ccpr));
}
}
void AGV_state_init(void)
{
float angle_offset = AGV_status.Direction;
if(AGV_status.runing_towards == 0 && angle_offset > 180)
{
angle_offset -= 360;
}
angle_offset -= AGV_status.runing_towards;
AGV_V_set(0.01);
if( angle_offset > 0)
{
if(angle_offset > 0.5)
{
motor_run(LEFT_MOTOR,CCW);
motor_run(RIGHT_MOTOR,CCW);
}
else
{
AGV_status.init_Directon_flag = 1;
}
}
else
{
if(angle_offset < -0.5)
{
motor_run(LEFT_MOTOR,CW);
motor_run(RIGHT_MOTOR,CW);
}
else
{
AGV_status.init_Directon_flag = 1;
}
}
if(AGV_status.init_Directon_flag)
{
motor_stop(LEFT_MOTOR);
motor_stop(RIGHT_MOTOR);
CON_ENCODE_LEFT->CNT = 0;
CON_ENCODE_RIGHT->CNT = 0;
}
}
void test(int16 m){
std::cout << "move motor to " << m << '\n';
int i = 0;
motor_run(m);
do{
isr_motor_step();
std::cout << ++i << ' ' << c32 << ' ' << c << '\n';
std::this_thread::sleep_for(std::chrono::microseconds(ccpr));
}while(run_flg);
}
int main(){
std::cout << "start test\n";
try{
initialize();
motor_run(100);
do{
std::this_thread::sleep_for(std::chrono::microseconds(ccpr));
isr_motor_step();
}while (run_flg);
// move motor to position 1000
motor_run(1000);
do{
std::this_thread::sleep_for(std::chrono::microseconds(ccpr));
isr_motor_step();
}while (run_flg);
// move back to position 0
motor_run(0);
do{
std::this_thread::sleep_for(std::chrono::microseconds(ccpr));
isr_motor_step();
}while (run_flg);
}
catch(std::exception & e){
std::cout << e.what() << '\n';
// we expect to trap an exception
return 0;
}
catch(...){
std::cout << "test interrupted\n";
return 1;
}
std::cout << "end test\n";
return 1;
}
int main(int argc, char *argv[]) {
struct motor motor1;
led_init(LED3);
button_init(BUTTON_USER);
motor_init(&motor1, GPIOD, MOTOR_ENABLE1, MOTOR_INPUT1, MOTOR_INPUT2);
motor_enable(&motor1);
/* Wait until button get pressed */
button_wait_set(BUTTON_USER);
motor_run(&motor1, MOTOR_RUN_RIGHT);
{
if (fault_detect()) {
fault_handle();
}
}
motor_stop(&motor1);
return 0;
}
