Mecanum robot firmware for WRS2020 Ususama
Dependencies: UsusamaSerial MecanumInterface PMSU_100 PIDController IMUInterface WaypointManager i2cmaster MecanumController
Diff: main.cpp
- Revision:
- 0:fe598340f74c
- Child:
- 1:2720d0e8b2f1
diff -r 000000000000 -r fe598340f74c main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Aug 23 17:12:44 2021 +0000 @@ -0,0 +1,121 @@ +#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(); + } + } +} \ No newline at end of file