Mecanum robot firmware for WRS2020 Ususama
Dependencies: UsusamaSerial MecanumInterface PMSU_100 PIDController IMUInterface WaypointManager i2cmaster MecanumController
main.cpp
- Committer:
- sgrsn
- Date:
- 2021-08-23
- Revision:
- 0:fe598340f74c
- Child:
- 1:2720d0e8b2f1
File content as of revision 0:fe598340f74c:
#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(0.1, 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() { int i = 0; while (true) { commander.receive_pc_process(); UsusamaProtocol::MoveCommand_t ref_pose = commander.getReferencePose(); leds = ref_pose.x % 16; i++; pc.writeData(ref_pose.x, UsusamaProtocol::COMMAND_POSE_X); pc.writeData(ref_pose.y, UsusamaProtocol::COMMAND_POSE_Y); pc.writeData(ref_pose.theta, UsusamaProtocol::COMMAND_POSE_THETA); ThisThread::sleep_for(53); } } void write_pc_thread() { while (true) { UsusamaProtocol::MoveCommand_t ref_pose = commander.getReferencePose(); pc.writeData(ref_pose.x, UsusamaProtocol::COMMAND_POSE_X); pc.writeData(ref_pose.y, UsusamaProtocol::COMMAND_POSE_Y); pc.writeData(ref_pose.theta, UsusamaProtocol::COMMAND_POSE_THETA); ThisThread::sleep_for(47); } } 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(); // 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; Timer t_way; t_way.start(); thread_read2pc.start(read_pc_thread); thread_write2pc.start(write_pc_thread); while(1) { // todo: read using timer thread_sleep_for(10); ususama.ComputePose(); ususama.ControlPosition( way_generator.GetPose(i_way) ); if(way_generator.GetPose(i_way).time_stamp < t_way.read()) { i_way++; } if( ususama.IsReferencePose( *(*ref_pose_itr) ) ) { //printf("goal\r\n"); ref_pose_itr++; if(ref_pose_itr == ref_pose_list.end()) { ref_pose_itr = ref_pose_list.begin(); } // 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; t_way.reset(); t_way.start(); } } }