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?
User | Revision | Line number | New 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 | } |