hidaka sato / Mbed OS WRS2021_mecanum_driver

Mecanum robot firmware for WRS2020 Ususama

Dependencies:   UsusamaSerial MecanumInterface PMSU_100 PIDController IMUInterface WaypointManager i2cmaster MecanumController

main.cpp@0:fe598340f74c, 2021-08-23 (annotated)

Committer:
sgrsn
Date:
Mon Aug 23 17:12:44 2021 +0000
Revision:
0:fe598340f74c
Child:
1:2720d0e8b2f1
Make Pose control with odometry and Communication with PC

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:fe598340f74c 1 #include "mbed.h"
sgrsn 0:fe598340f74c 2 #include "platform/mbed_thread.h"
sgrsn 0:fe598340f74c 3 #include "i2cmaster.h"
sgrsn 0:fe598340f74c 4 #include "mecanum_interface.hpp"
sgrsn 0:fe598340f74c 5 #include "imu_interface.hpp"
sgrsn 0:fe598340f74c 6 #include "mecanum_controller.hpp"
sgrsn 0:fe598340f74c 7 #include "waypoint_generator.hpp"
sgrsn 0:fe598340f74c 8 #include "ususama_serial.h"
sgrsn 0:fe598340f74c 9 #include "ususama_controller.hpp"
sgrsn 0:fe598340f74c 10 #include <vector>
sgrsn 0:fe598340f74c 11 #include <list>
sgrsn 0:fe598340f74c 12
sgrsn 0:fe598340f74c 13
sgrsn 0:fe598340f74c 14 i2c master(p28, p27);
sgrsn 0:fe598340f74c 15 MecanumI2C mecanum(&master);
sgrsn 0:fe598340f74c 16 PMSUInterface pmsu(p9, p10);
sgrsn 0:fe598340f74c 17 MecanumController ususama(&mecanum, &pmsu);
sgrsn 0:fe598340f74c 18
sgrsn 0:fe598340f74c 19 int32_t Register[12] = {};
sgrsn 0:fe598340f74c 20 UsusamaSerial pc(USBTX, USBRX, Register, 115200);
sgrsn 0:fe598340f74c 21 UsusamaController commander(&pc);
sgrsn 0:fe598340f74c 22 BusOut leds(LED1, LED2, LED3, LED4);
sgrsn 0:fe598340f74c 23
sgrsn 0:fe598340f74c 24 Thread thread_read2pc;
sgrsn 0:fe598340f74c 25 Thread thread_write2pc;
sgrsn 0:fe598340f74c 26
sgrsn 0:fe598340f74c 27
sgrsn 0:fe598340f74c 28 void mecanum_test()
sgrsn 0:fe598340f74c 29 {
sgrsn 0:fe598340f74c 30 ususama.ControllVelocity(Vel2D(0.1, 0.0, 0.0));
sgrsn 0:fe598340f74c 31 thread_sleep_for(1000);
sgrsn 0:fe598340f74c 32 ususama.ControllVelocity(Vel2D(0.0, 0.0, 0.0));
sgrsn 0:fe598340f74c 33 }
sgrsn 0:fe598340f74c 34
sgrsn 0:fe598340f74c 35 void imu_test()
sgrsn 0:fe598340f74c 36 {
sgrsn 0:fe598340f74c 37 for(int i = 0; i < 1000; i++)
sgrsn 0:fe598340f74c 38 {
sgrsn 0:fe598340f74c 39 printf("%d\r\n", (int)(pmsu.GetYawRadians() * RAD_TO_DEG));
sgrsn 0:fe598340f74c 40 }
sgrsn 0:fe598340f74c 41 }
sgrsn 0:fe598340f74c 42
sgrsn 0:fe598340f74c 43 void read_pc_thread()
sgrsn 0:fe598340f74c 44 {
sgrsn 0:fe598340f74c 45 int i = 0;
sgrsn 0:fe598340f74c 46 while (true) {
sgrsn 0:fe598340f74c 47 commander.receive_pc_process();
sgrsn 0:fe598340f74c 48 UsusamaProtocol::MoveCommand_t ref_pose = commander.getReferencePose();
sgrsn 0:fe598340f74c 49 leds = ref_pose.x % 16;
sgrsn 0:fe598340f74c 50 i++;
sgrsn 0:fe598340f74c 51 pc.writeData(ref_pose.x, UsusamaProtocol::COMMAND_POSE_X);
sgrsn 0:fe598340f74c 52 pc.writeData(ref_pose.y, UsusamaProtocol::COMMAND_POSE_Y);
sgrsn 0:fe598340f74c 53 pc.writeData(ref_pose.theta, UsusamaProtocol::COMMAND_POSE_THETA);
sgrsn 0:fe598340f74c 54 ThisThread::sleep_for(53);
sgrsn 0:fe598340f74c 55 }
sgrsn 0:fe598340f74c 56 }
sgrsn 0:fe598340f74c 57
sgrsn 0:fe598340f74c 58 void write_pc_thread()
sgrsn 0:fe598340f74c 59 {
sgrsn 0:fe598340f74c 60 while (true) {
sgrsn 0:fe598340f74c 61 UsusamaProtocol::MoveCommand_t ref_pose = commander.getReferencePose();
sgrsn 0:fe598340f74c 62 pc.writeData(ref_pose.x, UsusamaProtocol::COMMAND_POSE_X);
sgrsn 0:fe598340f74c 63 pc.writeData(ref_pose.y, UsusamaProtocol::COMMAND_POSE_Y);
sgrsn 0:fe598340f74c 64 pc.writeData(ref_pose.theta, UsusamaProtocol::COMMAND_POSE_THETA);
sgrsn 0:fe598340f74c 65 ThisThread::sleep_for(47);
sgrsn 0:fe598340f74c 66 }
sgrsn 0:fe598340f74c 67 }
sgrsn 0:fe598340f74c 68
sgrsn 0:fe598340f74c 69
sgrsn 0:fe598340f74c 70 int main()
sgrsn 0:fe598340f74c 71 {
sgrsn 0:fe598340f74c 72 std::list<std::unique_ptr<Pose2D>> ref_pose_list;
sgrsn 0:fe598340f74c 73 ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.8, 0.0));
sgrsn 0:fe598340f74c 74 ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.0, 0.0));
sgrsn 0:fe598340f74c 75 //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.5, 0));
sgrsn 0:fe598340f74c 76 //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.0, -1.57));
sgrsn 0:fe598340f74c 77 auto ref_pose_itr = ref_pose_list.begin();
sgrsn 0:fe598340f74c 78
sgrsn 0:fe598340f74c 79 // theta...
sgrsn 0:fe598340f74c 80 WaypointGenerator<Pose2D> way_generator( Pose2D(0, 0, 0), *(*ref_pose_itr), Pose2D(0.01, 0.01, 0.01), 5.0);
sgrsn 0:fe598340f74c 81 int i_way = 0;
sgrsn 0:fe598340f74c 82 Timer t_way;
sgrsn 0:fe598340f74c 83 t_way.start();
sgrsn 0:fe598340f74c 84
sgrsn 0:fe598340f74c 85 thread_read2pc.start(read_pc_thread);
sgrsn 0:fe598340f74c 86 thread_write2pc.start(write_pc_thread);
sgrsn 0:fe598340f74c 87
sgrsn 0:fe598340f74c 88 while(1)
sgrsn 0:fe598340f74c 89 {
sgrsn 0:fe598340f74c 90 // todo: read using timer
sgrsn 0:fe598340f74c 91 thread_sleep_for(10);
sgrsn 0:fe598340f74c 92
sgrsn 0:fe598340f74c 93 ususama.ComputePose();
sgrsn 0:fe598340f74c 94 ususama.ControlPosition( way_generator.GetPose(i_way) );
sgrsn 0:fe598340f74c 95
sgrsn 0:fe598340f74c 96 if(way_generator.GetPose(i_way).time_stamp < t_way.read())
sgrsn 0:fe598340f74c 97 {
sgrsn 0:fe598340f74c 98 i_way++;
sgrsn 0:fe598340f74c 99 }
sgrsn 0:fe598340f74c 100
sgrsn 0:fe598340f74c 101 if( ususama.IsReferencePose( *(*ref_pose_itr) ) )
sgrsn 0:fe598340f74c 102 {
sgrsn 0:fe598340f74c 103 //printf("goal\r\n");
sgrsn 0:fe598340f74c 104
sgrsn 0:fe598340f74c 105 ref_pose_itr++;
sgrsn 0:fe598340f74c 106 if(ref_pose_itr == ref_pose_list.end())
sgrsn 0:fe598340f74c 107 {
sgrsn 0:fe598340f74c 108 ref_pose_itr = ref_pose_list.begin();
sgrsn 0:fe598340f74c 109 }
sgrsn 0:fe598340f74c 110
sgrsn 0:fe598340f74c 111 // update waypoint
sgrsn 0:fe598340f74c 112 //printf("generate new waypoint...");
sgrsn 0:fe598340f74c 113 way_generator = WaypointGenerator<Pose2D>( ususama.GetPose(), *(*ref_pose_itr), Pose2D(0.01, 0.01, 0.005), 5.0);
sgrsn 0:fe598340f74c 114 //printf("has finished\r\n");
sgrsn 0:fe598340f74c 115
sgrsn 0:fe598340f74c 116 i_way = 0;
sgrsn 0:fe598340f74c 117 t_way.reset();
sgrsn 0:fe598340f74c 118 t_way.start();
sgrsn 0:fe598340f74c 119 }
sgrsn 0:fe598340f74c 120 }
sgrsn 0:fe598340f74c 121 }