Diff: main.cpp

Revision:

0:fe598340f74c

Child:

1:2720d0e8b2f1

--- /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