From 622da1c7840bb9dd2cd0cc72a65c74fe5733a94e Mon Sep 17 00:00:00 2001 From: Kristiyan Tsaklev Date: Sat, 10 Dec 2016 20:16:27 +0200 Subject: [PATCH 1/3] Update test --- src/Slim/logic/test.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/Slim/logic/test.cpp b/src/Slim/logic/test.cpp index 572b93c..6d1bc30 100644 --- a/src/Slim/logic/test.cpp +++ b/src/Slim/logic/test.cpp @@ -10,7 +10,18 @@ void initTest() { - enqueueLinearMovement(SPEED, 500); + static int moment = 0; + + switch(moment) + { + default: moment = 0; + case 0: enqueueLinearMovement(SPEED, 500); break; + case 1: enqueueLinearMovement(SPEED, -500); break; + case 2: enqueueTurn(SPEED, 0, 90); break; + case 3: enqueueTurn(SPEED, 0, -90); break; + } + + ++moment; return; enqueueLinearMovement(SPEED, 250); From 40303a59a80bbb5cc6aa1eae6ad8c028c28ca5eb Mon Sep 17 00:00:00 2001 From: Kristiyan Tsaklev Date: Sat, 10 Dec 2016 20:16:39 +0200 Subject: [PATCH 2/3] Left motor was put in reverse --- src/Slim/drivers/MotorDriver.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Slim/drivers/MotorDriver.c b/src/Slim/drivers/MotorDriver.c index a219346..825189c 100644 --- a/src/Slim/drivers/MotorDriver.c +++ b/src/Slim/drivers/MotorDriver.c @@ -33,6 +33,6 @@ void setMotor(int32_t speed, int dirPin, int speedPin) void setMotors(int32_t left, int32_t right) { - setMotor(left, MOTOR_LEFT_DIR_PIN, MOTOR_LEFT_PWM_PIN); + setMotor(-left, MOTOR_LEFT_DIR_PIN, MOTOR_LEFT_PWM_PIN); setMotor(right, MOTOR_RIGHT_DIR_PIN, MOTOR_RIGHT_PWM_PIN); } From b90149f311f9d4b62caa7373b8833a7f3b0566a3 Mon Sep 17 00:00:00 2001 From: Kristiyan Tsaklev Date: Sat, 10 Dec 2016 20:17:25 +0200 Subject: [PATCH 3/3] Rename indeces --- src/Slim/control/Movement.cpp | 36 +++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/src/Slim/control/Movement.cpp b/src/Slim/control/Movement.cpp index a3d65bb..135672e 100644 --- a/src/Slim/control/Movement.cpp +++ b/src/Slim/control/Movement.cpp @@ -13,10 +13,6 @@ #define MOTOR_KI 6.0f #define MOTOR_KD 0.0f -#define QUEUE_SIZE 64 -#define QUEUE_FRONT movement_queue[left_index] -#define QUEUE_BACK movement_queue[right_index] - static PidController left_motor_pid(MOTOR_KP, MOTOR_KI, MOTOR_KD, 255.0f / MOTOR_KI); static PidController right_motor_pid(MOTOR_KP, MOTOR_KI, MOTOR_KD, 255.0f / MOTOR_KI); @@ -28,21 +24,25 @@ struct Movement { int16_t right_target_speed; }; +#define QUEUE_SIZE 64 +#define QUEUE_FRONT movement_queue[queue_left] +#define QUEUE_BACK movement_queue[queue_right] + static Movement movement_queue[QUEUE_SIZE]; -int left_index = 0, right_index = 0; +int queue_left = 0, queue_right = 0; inline void push_queue() { - ++right_index; - if(right_index == QUEUE_SIZE) - right_index = 0; + ++queue_right; + if(queue_right == QUEUE_SIZE) + queue_right = 0; } inline void pop_queue() { - ++left_index; - if(left_index == QUEUE_SIZE) - left_index = 0; + ++queue_left; + if(queue_left == QUEUE_SIZE) + queue_left = 0; } void enqueueLinearMovement(int speed, int distance) @@ -100,7 +100,7 @@ void enqueueTurn(int speed, int turn_radius, int turn_degrees) void handleControlledMovement(float left_tire_speed, float right_tire_speed, float delta_time) { - if(left_index == right_index) + if(queue_left == queue_right) { // stop the motors, they should not be moving setMotors(0, 0); @@ -111,10 +111,10 @@ void handleControlledMovement(float left_tire_speed, float right_tire_speed, flo QUEUE_FRONT.expected_distance_left -= (float) left_tire_speed * delta_time; QUEUE_FRONT.expected_distance_right -= (float) right_tire_speed * delta_time; - float left_speed = left_motor_pid.sample(QUEUE_FRONT.left_target_speed, left_tire_speed, delta_time); - float right_speed = right_motor_pid.sample(QUEUE_FRONT.right_target_speed, right_tire_speed, delta_time); + float left_power = left_motor_pid.sample(QUEUE_FRONT.left_target_speed, left_tire_speed, delta_time); + float right_power = right_motor_pid.sample(QUEUE_FRONT.right_target_speed, right_tire_speed, delta_time); - setMotors(left_speed, right_speed); + setMotors(left_power, right_power); if( (QUEUE_FRONT.expected_distance_left < 0 && QUEUE_FRONT.left_target_speed > 0) || (QUEUE_FRONT.expected_distance_left > 0 && QUEUE_FRONT.left_target_speed < 0) || @@ -124,7 +124,7 @@ void handleControlledMovement(float left_tire_speed, float right_tire_speed, flo pop_queue(); } - if(left_index == right_index) + if(queue_left == queue_right) { setMotors(0, 0); left_motor_pid.zero(); @@ -134,7 +134,7 @@ void handleControlledMovement(float left_tire_speed, float right_tire_speed, flo int isQueueEmpty() { - return left_index == right_index; + return queue_left == queue_right; } void clearQueue() @@ -142,5 +142,5 @@ void clearQueue() setMotors(0, 0); left_motor_pid.zero(); right_motor_pid.zero(); - right_index = left_index; + queue_right = queue_left; }