Mecanum robot firmware for WRS2020 Ususama
Dependencies: UsusamaSerial MecanumInterface PMSU_100 PIDController IMUInterface WaypointManager i2cmaster MecanumController
Diff: main.cpp
- Revision:
- 2:626a20dd7987
- Parent:
- 1:2720d0e8b2f1
- Child:
- 3:35244995ff2e
diff -r 2720d0e8b2f1 -r 626a20dd7987 main.cpp --- a/main.cpp Tue Aug 24 07:06:27 2021 +0000 +++ b/main.cpp Wed Aug 25 08:56:53 2021 +0000 @@ -27,7 +27,7 @@ void mecanum_test() { - ususama.ControllVelocity(Vel2D(0.1, 0.0, 0.0)); + ususama.ControllVelocity(Vel2D(1.0, 0.0, 0.0)); thread_sleep_for(1000); ususama.ControllVelocity(Vel2D(0.0, 0.0, 0.0)); } @@ -44,75 +44,112 @@ { while (true) { commander.receive_pc_process(); - leds = commander.getReferencePose().x; ThisThread::sleep_for(53); } } void write_pc_thread() { - Pose2D current_pose(0,0,0); while (true) { - current_pose = ususama.GetPose(); - commander.write_robot_state(current_pose.x, current_pose.y, current_pose.theta); - ThisThread::sleep_for(47); + commander.write_robot_state(); + ThisThread::sleep_for(101); } } int main() { - std::list<std::unique_ptr<Pose2D>> ref_pose_list; - ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.8, 0.0)); - ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.0, 0.0)); + //std::list<std::unique_ptr<Pose2D>> ref_pose_list; + //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.8, 0.0)); + //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.0, 0.0)); //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.5, 0)); //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.0, -1.57)); - auto ref_pose_itr = ref_pose_list.begin(); + //auto ref_pose_itr = ref_pose_list.begin(); + ususama.ControllVelocity(Vel2D(0.0, 0.0, 0.0)); // theta... - WaypointGenerator<Pose2D> way_generator( Pose2D(0, 0, 0), *(*ref_pose_itr), Pose2D(0.01, 0.01, 0.01), 5.0); - int i_way = 0; + //WaypointGenerator<Pose2D> way_generator( Pose2D(0, 0, 0), Pose2D(0.8, 0, 0), Pose2D(0.01, 0.01, 0.01), 5.0); + WaypointGenerator<Pose2D> way_generator( Pose2D(0, 0, 0), Pose2D(0.8, 0, 0), 10.0); + //int i_way = 0; Timer t_way; t_way.start(); thread_read2pc.start(read_pc_thread); thread_write2pc.start(write_pc_thread); + // to do waypoint生成とmove_command.enableの処理 + bool is_way_generated = false; + bool is_stoped = false; + while(1) { - // todo: read using timer - thread_sleep_for(10); - - - UsusamaProtocol::MoveCommand_t ref_pose = commander.getReferencePose(); - + //thread_sleep_for(10); ususama.ComputePose(); - ususama.ControlPosition( way_generator.GetPose(i_way) ); + commander.update(ususama.GetPose().x, ususama.GetPose().y, ususama.GetPose().theta); + + UsusamaProtocol::MoveCommand_t move_command = commander.getMoveCommand(); + Pose2D ref_pose(move_command.x, move_command.y, move_command.theta); - if(way_generator.GetPose(i_way).time_stamp < t_way.read()) + if(!move_command.enable) { - i_way++; + ususama.ControllVelocity(Vel2D(0.0, 0.0, 0.0)); + continue; } - if( ususama.IsReferencePose( *(*ref_pose_itr) ) ) + // waypoint存在していれば移動 + if(is_way_generated) { - //printf("goal\r\n"); - - ref_pose_itr++; - if(ref_pose_itr == ref_pose_list.end()) + /*if(way_generator.GetPose(i_way).time_stamp < t_way.read()) { - ref_pose_itr = ref_pose_list.begin(); + i_way++; } - - // update waypoint - //printf("generate new waypoint..."); - way_generator = WaypointGenerator<Pose2D>( ususama.GetPose(), *(*ref_pose_itr), Pose2D(0.01, 0.01, 0.005), 5.0); - //printf("has finished\r\n"); - - i_way = 0; + ususama.ControlPosition( way_generator.GetPose(i_way) );*/ + float t = t_way.read(); + ususama.ControlPosition( way_generator.GetPose( t ) ); + } + // waypoint存在していなければ生成 + else + { + way_generator = WaypointGenerator<Pose2D>( + ususama.GetPose(), + ref_pose, + //Pose2D(0.05, 0.05, 0.1), + 10.0 + ); + is_way_generated = true; + //i_way = 0; t_way.reset(); t_way.start(); } + + // stopコマンド確認 + if( commander.is_stop_called() ) + { + is_way_generated = false; + commander.disable_to_move(); + t_way.stop(); + is_stoped = true; + //ususama.ControllVelocity( Vel2D(0, 0, 0) ); + // ususama.CoastAllMotor(); + // ususama.ControlPosition( ususama.GetPose() ); + } + else if(is_stoped) + { + t_way.start(); + is_stoped = false; + } + + + if( ususama.IsReferencePose( ref_pose ) ) + { + is_way_generated = false; + commander.disable_to_move(); + commander.setReachedGoal(true); + } + else + { + commander.setReachedGoal(false); + } } } \ No newline at end of file