Skip to content

Commit 63bad05

Browse files
Coreyboy1820Coreyboy1820
authored
Rmd demo update (#91)
* ✨ updated rmd-demo and fixed the comments in the drive serial * ❇️ added demos and main control loops back
1 parent 7a02d93 commit 63bad05

File tree

3 files changed

+85
-156
lines changed

3 files changed

+85
-156
lines changed

‎code/CMakeLists.txt

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@ find_package(libhal-util REQUIRED CONFIG)
1313
find_package(libhal-pca REQUIRED CONFIG)
1414

1515
set(SUBSYSTEMS drive drive_serial arm arm_serial science)
16-
set(DEMOS co2_sensor_demo)
16+
set(DEMOS co2_sensor_demo rmd_demo)
1717
set(TARGETS lpc4078_10mhz lpc4078_12mhz)
1818

1919
foreach(target IN LISTS TARGETS)

‎code/applications/control_loops/drive_serial.cpp

Lines changed: 25 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -36,25 +36,25 @@ hal::status application(sjsu::hardware_map& p_map)
3636

3737
auto can_router = hal::can_router::create(can).value();
3838

39-
// auto left_steer_motor =
40-
// HAL_CHECK(hal::rmd::drc::create(can_router, clock, 6.0, 0x141));
41-
// auto left_hub_motor =
42-
// HAL_CHECK(hal::rmd::drc::create(can_router, clock, 15.0, 0x142));
43-
// auto back_steer_motor =
44-
// HAL_CHECK(hal::rmd::drc::create(can_router, clock, 6.0, 0x145));
45-
// auto back_hub_motor =
46-
// HAL_CHECK(hal::rmd::drc::create(can_router, clock, 15.0, 0x146));
47-
// auto right_steer_motor =
48-
// HAL_CHECK(hal::rmd::drc::create(can_router, clock, 6.0, 0x143));
49-
// auto right_hub_motor =
50-
// HAL_CHECK(hal::rmd::drc::create(can_router, clock, 15.0, 0x144));
51-
52-
// Drive::tri_wheel_router::leg right(
53-
// right_steer_motor, right_hub_motor, magnet0);
54-
// Drive::tri_wheel_router::leg left(left_steer_motor, left_hub_motor, magnet2);
55-
// Drive::tri_wheel_router::leg back(back_steer_motor, back_hub_motor, magnet1);
56-
57-
// Drive::tri_wheel_router tri_wheel{ right, left, back };
39+
auto left_steer_motor =
40+
HAL_CHECK(hal::rmd::drc::create(can_router, clock, 6.0, 0x141));
41+
auto left_hub_motor =
42+
HAL_CHECK(hal::rmd::drc::create(can_router, clock, 15.0, 0x142));
43+
auto back_steer_motor =
44+
HAL_CHECK(hal::rmd::drc::create(can_router, clock, 6.0, 0x145));
45+
auto back_hub_motor =
46+
HAL_CHECK(hal::rmd::drc::create(can_router, clock, 15.0, 0x146));
47+
auto right_steer_motor =
48+
HAL_CHECK(hal::rmd::drc::create(can_router, clock, 6.0, 0x143));
49+
auto right_hub_motor =
50+
HAL_CHECK(hal::rmd::drc::create(can_router, clock, 15.0, 0x144));
51+
52+
Drive::tri_wheel_router::leg right(
53+
right_steer_motor, right_hub_motor, magnet0);
54+
Drive::tri_wheel_router::leg left(left_steer_motor, left_hub_motor, magnet2);
55+
Drive::tri_wheel_router::leg back(back_steer_motor, back_hub_motor, magnet1);
56+
57+
Drive::tri_wheel_router tri_wheel{ right, left, back };
5858
Drive::drive_commands commands;
5959
Drive::motor_feedback motor_speeds;
6060
Drive::tri_wheel_router_arguments arguments;
@@ -66,7 +66,7 @@ hal::status application(sjsu::hardware_map& p_map)
6666
};
6767

6868
HAL_CHECK(hal::delay(clock, 1000ms));
69-
// tri_wheel.home(clock);
69+
tri_wheel.home(clock);
7070
HAL_CHECK(hal::delay(clock, 1000ms));
7171
HAL_CHECK(hal::write(terminal, "Starting control loop..."));
7272

@@ -87,15 +87,13 @@ hal::status application(sjsu::hardware_map& p_map)
8787
}
8888
// end of serial
8989
commands = validate_commands(commands);
90-
// commands = mode_switcher.switch_steer_mode(
91-
// commands, arguments, motor_speeds, terminal);
92-
// commands.speed = lerp.lerp(commands.speed);
90+
commands = mode_switcher.switch_steer_mode(commands, arguments, motor_speeds, terminal);
91+
commands.speed = lerp.lerp(commands.speed);
9392

94-
commands.print(terminal);
95-
// arguments = select_mode(commands);
96-
// HAL_CHECK(tri_wheel.move(arguments, clock));
93+
arguments = select_mode(commands);
94+
HAL_CHECK(tri_wheel.move(arguments, clock));
9795

98-
// motor_speeds = HAL_CHECK(tri_wheel.get_motor_feedback(clock));
96+
motor_speeds = HAL_CHECK(tri_wheel.get_motor_feedback(clock));
9997
HAL_CHECK(hal::delay(clock, 30ms));
10098
}
10199

‎code/applications/demos/rmd_demo.cpp

Lines changed: 59 additions & 128 deletions
Original file line numberDiff line numberDiff line change
@@ -1,128 +1,59 @@
1-
// #include <cinttypes>
2-
3-
// #include <libhal-esp8266/at/socket.hpp>
4-
// #include <libhal-esp8266/at/wlan_client.hpp>
5-
// #include <libhal-esp8266/util.hpp>
6-
// #include <libhal-rmd/drc.hpp>
7-
// #include <libhal-util/serial.hpp>
8-
// #include <libhal-util/steady_clock.hpp>
9-
// #include <libhal/can.hpp>
10-
11-
// #include "../dto/drive_dto.hpp"
12-
// #include "../hardware_map.hpp"
13-
// #include "../drive/implementation/mission_control_handler.hpp"
14-
// #include "common/util.hpp"
15-
16-
// hal::status application(sjsu::hardware_map& p_map)
17-
// {
18-
// using namespace std::chrono_literals;
19-
// using namespace hal::literals;
20-
// Drive::drive_commands commands;
21-
// Drive::MissionControlHandler mission_control;
22-
// auto& can = *p_map.can;
23-
// auto& terminal = *p_map.terminal;
24-
// auto& clock = *p_map.steady_clock;
25-
// float encoderData = 0.0f;
26-
// auto& esp = *p_map.esp;
27-
28-
// // HAL_CHECK(hal::write(terminal, "Starting RMD + WiFi Demo...\n"));
29-
// HAL_CHECK(hal::delay(clock, 1s));
30-
31-
// auto router = HAL_CHECK(hal::can_router::create(can));
32-
// auto left_hub_motor =
33-
// HAL_CHECK(hal::rmd::drc::create(router, clock, 15.0, 0x142));
34-
// // auto right_hub_motor = HAL_CHECK(hal::rmd::drc::create(router, 15.0,
35-
// // 0x144)); auto back_hub_motor =
36-
// // HAL_CHECK(hal::rmd::drc::create(router, 15.0, 0x146));
37-
38-
// // HAL_CHECK(hal::write(terminal, "Connecting to wifi...\n"));
39-
// // std::array<hal::byte, 8192> buffer{};
40-
// // static std::string_view get_request = "";
41-
42-
// // auto wifi_result = hal::esp8266::at::wlan_client::create(
43-
// // esp,
44-
// // "SJSU Robotics 2.4GHz",
45-
// // "R0Bot1cs3250",
46-
// // HAL_CHECK(hal::create_timeout(*p_map.steady_clock, 10s)));
47-
48-
// // if (!wifi_result) {
49-
// // HAL_CHECK(hal::write(terminal, "Failed to create wifi client!\n"));
50-
// // return wifi_result.error();
51-
// // }
52-
53-
// // auto wifi = wifi_result.value();
54-
55-
// // auto socket_result = hal::esp8266::at::socket::create(
56-
// // wifi,
57-
// // HAL_CHECK(hal::create_timeout(*p_map.steady_clock, 1s)),
58-
// // {
59-
// // .type = hal::socket::type::tcp,
60-
// // .domain = "192.168.1.183",
61-
// // .port = "5000",
62-
// // });
63-
64-
// // if (!socket_result) {
65-
// // HAL_CHECK(hal::write(terminal, "TCP Socket couldn't be
66-
// established\n"));
67-
// // return socket_result.error();
68-
// // }
69-
70-
// // auto socket = std::move(socket_result.value());
71-
// // HAL_CHECK(hal::write(
72-
// // terminal,left_hub_motor
73-
// // "Connected to wifi! Starting main control loop in 1 second...\n"));
74-
// HAL_CHECK(hal::delay(*p_map.steady_clock, 1s));
75-
// HAL_CHECK(left_hub_motor.system_control(hal::rmd::drc::system::running));
76-
// // right_hub_motor.system_control(hal::rmd::drc::system::running);
77-
// // back_hub_motor.system_control(hal::rmd::drc::system::running);
78-
79-
// HAL_CHECK(hal::delay(*p_map.steady_clock, 1s));
80-
81-
// HAL_CHECK(hal::write(terminal, "Starting!\n"));
82-
83-
// HAL_CHECK(left_hub_motor.position_control(30.0_deg, 5.0_rpm));
84-
85-
// while (true) {
86-
// HAL_CHECK(hal::delay(*p_map.steady_clock, 1s))
87-
88-
// //
89-
// HAL_CHECK(left_hub_motor.feedback_request(hal::rmd::drc::read::encoder_data));
90-
// // hal::print<20>(terminal, "%f\n", encoderData);
91-
// // HAL_CHECK(hal::write(terminal, encoder));
92-
// // buffer.fill('.');
93-
// // get_request = "GET /drive" + get_rover_status() +
94-
// // " HTTP/1.1\r\n"
95-
// // "Host: 192.168.1.183:5000/\r\n"
96-
// // "\r\n";
97-
98-
// // auto write_result =
99-
// // socket.write(hal::as_bytes(get_request),
100-
// // HAL_CHECK(hal::create_timeout(*p_map.steady_clock,
101-
// // 500ms)));
102-
// // if (!write_result) {
103-
// // continue;
104-
// // }
105-
// HAL_CHECK(hal::delay(*p_map.steady_clock, 1s));
106-
// // auto received = HAL_CHECK(socket.read(buffer)).data;
107-
// // auto result = to_string_view(received);
108-
// // auto start = result.find('{');
109-
// // auto end = result.find('}');
110-
// // auto json = result.substr(start, end - start + 1);
111-
112-
// // HAL_CHECK(hal::write(terminal, "Received:\n"));
113-
// // HAL_CHECK(hal::write(terminal, json));
114-
// // HAL_CHECK(hal::write(terminal, "\r\n\n"));
115-
116-
// // std::string json_string(json);
117-
// // commands =
118-
// // HAL_CHECK(mission_control.ParseMissionControlData(json_string,
119-
// // terminal));
120-
121-
// // auto rmd_speed = commands.speed;
122-
// // HAL_CHECK(hal::delay(*p_map.steady_clock, 1s));
123-
// // right_hub_motor.velocity_control(rmd_speed);
124-
// // back_hub_motor.velocity_control(rmd_speed);
125-
// }
126-
127-
// return hal::success();
128-
// }
1+
#include <cinttypes>
2+
3+
#include <libhal-esp8266/at/socket.hpp>
4+
#include <libhal-esp8266/at/wlan_client.hpp>
5+
#include <libhal-esp8266/util.hpp>
6+
#include <libhal-rmd/drc.hpp>
7+
#include <libhal-util/serial.hpp>
8+
#include <libhal-util/steady_clock.hpp>
9+
#include <libhal/can.hpp>
10+
11+
#include "drive/dto/drive_dto.hpp"
12+
#include "hardware_map.hpp"
13+
#include "drive/implementation/mission_control_handler.hpp"
14+
#include "common/util.hpp"
15+
16+
hal::status application(sjsu::hardware_map& p_map)
17+
{
18+
using namespace std::chrono_literals;
19+
using namespace hal::literals;
20+
Drive::drive_commands commands;
21+
auto& can = *p_map.can;
22+
auto& terminal = *p_map.terminal;
23+
auto& clock = *p_map.steady_clock;
24+
auto& esp = *p_map.esp;
25+
26+
HAL_CHECK(hal::delay(clock, 1s));
27+
28+
auto router = HAL_CHECK(hal::can_router::create(can));
29+
30+
auto left_steer_motor =
31+
HAL_CHECK(hal::rmd::drc::create(router, clock, 15.0, 0x141));
32+
auto right_steer_motor =
33+
HAL_CHECK(hal::rmd::drc::create(router, clock, 15.0, 0x143));
34+
auto back_steer_motor =
35+
HAL_CHECK(hal::rmd::drc::create(router, clock, 15.0, 0x145));
36+
auto left_hub_motor =
37+
HAL_CHECK(hal::rmd::drc::create(router, clock, 15.0, 0x142));
38+
auto right_hub_motor =
39+
HAL_CHECK(hal::rmd::drc::create(router, clock, 15.0, 0x144));
40+
auto back_hub_motor =
41+
HAL_CHECK(hal::rmd::drc::create(router, clock, 15.0, 0x146));
42+
43+
HAL_CHECK(hal::delay(*p_map.steady_clock, 1s));
44+
45+
HAL_CHECK(hal::write(terminal, "Starting!\n"));
46+
47+
left_steer_motor.velocity_control(5.0_rpm);
48+
right_steer_motor.velocity_control(5.0_rpm);
49+
back_steer_motor.velocity_control(5.0_rpm);
50+
left_hub_motor.velocity_control(5.0_rpm);
51+
right_hub_motor.velocity_control(5.0_rpm);
52+
back_hub_motor.velocity_control(5.0_rpm);
53+
54+
while (true) {
55+
HAL_CHECK(hal::delay(clock, 1s));
56+
}
57+
58+
return hal::success();
59+
}

0 commit comments

Comments
 (0)