Mecanum robot firmware for WRS2020 Ususama
Dependencies: UsusamaSerial MecanumInterface PMSU_100 PIDController IMUInterface WaypointManager i2cmaster MecanumController
main.cpp
- Committer:
- sgrsn
- Date:
- 2021-08-25
- Revision:
- 3:35244995ff2e
- Parent:
- 2:626a20dd7987
File content as of revision 3:35244995ff2e:
#include "mbed.h" #include "platform/mbed_thread.h" #include "i2cmaster.h" #include "mecanum_interface.hpp" #include "imu_interface.hpp" #include "mecanum_controller.hpp" #include "waypoint_generator.hpp" #include "ususama_serial.h" #include "ususama_controller.hpp" #include <vector> #include <list> i2c master(p28, p27); MecanumI2C mecanum(&master); PMSUInterface pmsu(p9, p10); MecanumController ususama(&mecanum, &pmsu); int32_t Register[12] = {}; UsusamaSerial pc(USBTX, USBRX, Register, 115200); UsusamaController commander(&pc); BusOut leds(LED1, LED2, LED3, LED4); Thread thread_read2pc; Thread thread_write2pc; void mecanum_test() { ususama.ControllVelocity(Vel2D(1.0, 0.0, 0.0)); thread_sleep_for(1000); ususama.ControllVelocity(Vel2D(0.0, 0.0, 0.0)); } void imu_test() { for(int i = 0; i < 1000; i++) { printf("%d\r\n", (int)(pmsu.GetYawRadians() * RAD_TO_DEG)); } } void read_pc_thread() { while (true) { commander.receive_pc_process(); ThisThread::sleep_for(103); } } void write_pc_thread() { while (true) { commander.write_robot_state(); ThisThread::sleep_for(211); } } 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)); //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(); ususama.ControllVelocity(Vel2D(0.0, 0.0, 0.0)); // theta... WaypointGenerator<Pose2D> way_generator( Pose2D(0, 0, 0), Pose2D(0.8, 0, 0), 10.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; bool new_goal = false; while(1) { //thread_sleep_for(10); ususama.ComputePose(); 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(!move_command.enable) { ususama.ControllVelocity(Vel2D(0.0, 0.0, 0.0)); continue; } if( commander.is_ref_pose_changed() || !is_way_generated) { way_generator = WaypointGenerator<Pose2D>( ususama.GetPose(), ref_pose, 10.0 ); is_way_generated = true; commander.notify_use_ref_pose(); t_way.reset(); t_way.start(); } // waypoint存在していれば移動 else { float t = t_way.read(); ususama.ControlPosition( way_generator.GetPose( t ) ); } // 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); } } }