From d90e46c9350219bd94af2fad60d5389c7bcdb848 Mon Sep 17 00:00:00 2001
From: Robert Hedman <robert.hedman@mac.com>
Date: Thu, 30 Nov 2017 00:03:52 +0100
Subject: [PATCH] updated kinematic function
---
src/main.rs | 4 +++-
1 file changed, 3 insertions(+), 1 deletion(-)
diff --git a/src/main.rs b/src/main.rs
index 6353f2b..07854d9 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -275,7 +275,9 @@ fn tick_to_speed(tick: i32) -> f32 {
tick/10.0
}
fn update_state(pos: &(f32,f32,f32), l_speed: f32, r_speed: f32) {
- pos.0 += 0.5;
+ pos.0 += 0.5*(l_speed + r_speed)*((pos.2).to_radians().sin());
+ pos.0 += 0.5*(l_speed + r_speed)*((pos.2).to_radians().cos());
+ pos.2 += (r_speed - l_speed)/AXEL_WIDTH;
}
fn update_map_with_lidar(lidar_data: [u8; LIDAR_BUF_SIZE], message_len: usize, map: &mut [[u32; ROWS]; COLS]) {
map[0][0] += 1;
--
GitLab