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@2:626a20dd7987, 2021-08-25 (annotated)

Committer:
sgrsn
Date:
Wed Aug 25 08:56:53 2021 +0000
Revision:
2:626a20dd7987
Parent:
1:2720d0e8b2f1
Child:
3:35244995ff2e
Fix parameter, Update waypoint generation, Update ususama protocol

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 2:626a20dd7987 30 ususama.ControllVelocity(Vel2D(1.0, 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 while (true) {
sgrsn 0:fe598340f74c 46 commander.receive_pc_process();
sgrsn 0:fe598340f74c 47 ThisThread::sleep_for(53);
sgrsn 0:fe598340f74c 48 }
sgrsn 0:fe598340f74c 49 }
sgrsn 0:fe598340f74c 50
sgrsn 0:fe598340f74c 51 void write_pc_thread()
sgrsn 0:fe598340f74c 52 {
sgrsn 0:fe598340f74c 53 while (true) {
sgrsn 2:626a20dd7987 54 commander.write_robot_state();
sgrsn 2:626a20dd7987 55 ThisThread::sleep_for(101);
sgrsn 0:fe598340f74c 56 }
sgrsn 0:fe598340f74c 57 }
sgrsn 0:fe598340f74c 58
sgrsn 0:fe598340f74c 59
sgrsn 0:fe598340f74c 60 int main()
sgrsn 0:fe598340f74c 61 {
sgrsn 2:626a20dd7987 62 //std::list<std::unique_ptr<Pose2D>> ref_pose_list;
sgrsn 2:626a20dd7987 63 //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.8, 0.0));
sgrsn 2:626a20dd7987 64 //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.0, 0.0));
sgrsn 0:fe598340f74c 65 //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.5, 0));
sgrsn 0:fe598340f74c 66 //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.0, -1.57));
sgrsn 2:626a20dd7987 67 //auto ref_pose_itr = ref_pose_list.begin();
sgrsn 0:fe598340f74c 68
sgrsn 2:626a20dd7987 69 ususama.ControllVelocity(Vel2D(0.0, 0.0, 0.0));
sgrsn 0:fe598340f74c 70 // theta...
sgrsn 2:626a20dd7987 71 //WaypointGenerator<Pose2D> way_generator( Pose2D(0, 0, 0), Pose2D(0.8, 0, 0), Pose2D(0.01, 0.01, 0.01), 5.0);
sgrsn 2:626a20dd7987 72 WaypointGenerator<Pose2D> way_generator( Pose2D(0, 0, 0), Pose2D(0.8, 0, 0), 10.0);
sgrsn 2:626a20dd7987 73 //int i_way = 0;
sgrsn 0:fe598340f74c 74 Timer t_way;
sgrsn 0:fe598340f74c 75 t_way.start();
sgrsn 0:fe598340f74c 76
sgrsn 0:fe598340f74c 77 thread_read2pc.start(read_pc_thread);
sgrsn 0:fe598340f74c 78 thread_write2pc.start(write_pc_thread);
sgrsn 0:fe598340f74c 79
sgrsn 2:626a20dd7987 80 // to do waypoint生成とmove_command.enableの処理
sgrsn 2:626a20dd7987 81 bool is_way_generated = false;
sgrsn 2:626a20dd7987 82 bool is_stoped = false;
sgrsn 2:626a20dd7987 83
sgrsn 0:fe598340f74c 84 while(1)
sgrsn 0:fe598340f74c 85 {
sgrsn 2:626a20dd7987 86 //thread_sleep_for(10);
sgrsn 1:2720d0e8b2f1 87
sgrsn 0:fe598340f74c 88 ususama.ComputePose();
sgrsn 2:626a20dd7987 89 commander.update(ususama.GetPose().x, ususama.GetPose().y, ususama.GetPose().theta);
sgrsn 2:626a20dd7987 90
sgrsn 2:626a20dd7987 91 UsusamaProtocol::MoveCommand_t move_command = commander.getMoveCommand();
sgrsn 2:626a20dd7987 92 Pose2D ref_pose(move_command.x, move_command.y, move_command.theta);
sgrsn 0:fe598340f74c 93
sgrsn 2:626a20dd7987 94 if(!move_command.enable)
sgrsn 0:fe598340f74c 95 {
sgrsn 2:626a20dd7987 96 ususama.ControllVelocity(Vel2D(0.0, 0.0, 0.0));
sgrsn 2:626a20dd7987 97 continue;
sgrsn 0:fe598340f74c 98 }
sgrsn 0:fe598340f74c 99
sgrsn 2:626a20dd7987 100 // waypoint存在していれば移動
sgrsn 2:626a20dd7987 101 if(is_way_generated)
sgrsn 0:fe598340f74c 102 {
sgrsn 2:626a20dd7987 103 /*if(way_generator.GetPose(i_way).time_stamp < t_way.read())
sgrsn 0:fe598340f74c 104 {
sgrsn 2:626a20dd7987 105 i_way++;
sgrsn 0:fe598340f74c 106 }
sgrsn 2:626a20dd7987 107 ususama.ControlPosition( way_generator.GetPose(i_way) );*/
sgrsn 2:626a20dd7987 108 float t = t_way.read();
sgrsn 2:626a20dd7987 109 ususama.ControlPosition( way_generator.GetPose( t ) );
sgrsn 2:626a20dd7987 110 }
sgrsn 2:626a20dd7987 111 // waypoint存在していなければ生成
sgrsn 2:626a20dd7987 112 else
sgrsn 2:626a20dd7987 113 {
sgrsn 2:626a20dd7987 114 way_generator = WaypointGenerator<Pose2D>(
sgrsn 2:626a20dd7987 115 ususama.GetPose(),
sgrsn 2:626a20dd7987 116 ref_pose,
sgrsn 2:626a20dd7987 117 //Pose2D(0.05, 0.05, 0.1),
sgrsn 2:626a20dd7987 118 10.0
sgrsn 2:626a20dd7987 119 );
sgrsn 2:626a20dd7987 120 is_way_generated = true;
sgrsn 2:626a20dd7987 121 //i_way = 0;
sgrsn 0:fe598340f74c 122 t_way.reset();
sgrsn 0:fe598340f74c 123 t_way.start();
sgrsn 0:fe598340f74c 124 }
sgrsn 2:626a20dd7987 125
sgrsn 2:626a20dd7987 126 // stopコマンド確認
sgrsn 2:626a20dd7987 127 if( commander.is_stop_called() )
sgrsn 2:626a20dd7987 128 {
sgrsn 2:626a20dd7987 129 is_way_generated = false;
sgrsn 2:626a20dd7987 130 commander.disable_to_move();
sgrsn 2:626a20dd7987 131 t_way.stop();
sgrsn 2:626a20dd7987 132 is_stoped = true;
sgrsn 2:626a20dd7987 133 //ususama.ControllVelocity( Vel2D(0, 0, 0) );
sgrsn 2:626a20dd7987 134 // ususama.CoastAllMotor();
sgrsn 2:626a20dd7987 135 // ususama.ControlPosition( ususama.GetPose() );
sgrsn 2:626a20dd7987 136 }
sgrsn 2:626a20dd7987 137 else if(is_stoped)
sgrsn 2:626a20dd7987 138 {
sgrsn 2:626a20dd7987 139 t_way.start();
sgrsn 2:626a20dd7987 140 is_stoped = false;
sgrsn 2:626a20dd7987 141 }
sgrsn 2:626a20dd7987 142
sgrsn 2:626a20dd7987 143
sgrsn 2:626a20dd7987 144 if( ususama.IsReferencePose( ref_pose ) )
sgrsn 2:626a20dd7987 145 {
sgrsn 2:626a20dd7987 146 is_way_generated = false;
sgrsn 2:626a20dd7987 147 commander.disable_to_move();
sgrsn 2:626a20dd7987 148 commander.setReachedGoal(true);
sgrsn 2:626a20dd7987 149 }
sgrsn 2:626a20dd7987 150 else
sgrsn 2:626a20dd7987 151 {
sgrsn 2:626a20dd7987 152 commander.setReachedGoal(false);
sgrsn 2:626a20dd7987 153 }
sgrsn 0:fe598340f74c 154 }
sgrsn 0:fe598340f74c 155 }