devoalda
/
INF2004_Project
mirror of https://github.com/L4ncelot-R/freeRTOS-car.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

re structured

This commit is contained in:
Richie 2023-10-20 21:26:48 +08:00
parent 69bb805846
commit ea9318b747
15 changed files with 560 additions and 302 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

136
.clang-format Normal file
View File

@ -0,0 +1,136 @@
---
Language: Cpp
AccessModifierOffset: -4
AlignAfterOpenBracket: Align
AlignConsecutiveMacros: true
AlignConsecutiveAssignments: true
AlignConsecutiveDeclarations: true
AlignEscapedNewlines: Left
AlignOperands: true
AlignTrailingComments: true
AllowAllArgumentsOnNextLine: true
AllowAllConstructorInitializersOnNextLine: true
AllowAllParametersOfDeclarationOnNextLine: true
AllowShortBlocksOnASingleLine: Never
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: None
AllowShortLambdasOnASingleLine: All
AllowShortIfStatementsOnASingleLine: Never
AllowShortLoopsOnASingleLine: false
AlwaysBreakAfterDefinitionReturnType: TopLevel
AlwaysBreakAfterReturnType: TopLevelDefinitions
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: Yes
BinPackArguments: false
BinPackParameters: false
BraceWrapping:
AfterCaseLabel: false
AfterClass: true
AfterControlStatement: true
AfterEnum: true
AfterFunction: true
AfterNamespace: false
AfterObjCDeclaration: true
AfterStruct: true
AfterUnion: true
AfterExternBlock: true
BeforeCatch: true
BeforeElse: true
IndentBraces: false
SplitEmptyFunction: true
SplitEmptyRecord: true
SplitEmptyNamespace: true
BreakBeforeBinaryOperators: All
BreakBeforeBraces: Custom
BreakBeforeInheritanceComma: false
BreakInheritanceList: BeforeComma
BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: false
BreakConstructorInitializers: BeforeComma
BreakAfterJavaFieldAnnotations: true
BreakStringLiterals: true
ColumnLimit: 80
CommentPragmas: '^ IWYU pragma:'
CompactNamespaces: false
ConstructorInitializerAllOnOneLineOrOnePerLine: false
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 4
Cpp11BracedListStyle: false
DeriveLineEnding: true
DerivePointerAlignment: false
DisableFormat: false
ExperimentalAutoDetectBinPacking: false
FixNamespaceComments: false
ForEachMacros:
- foreach
- Q_FOREACH
- BOOST_FOREACH
IncludeBlocks: Preserve
IncludeCategories:
- Regex: '^"(llvm|llvm-c|clang|clang-c)/'
Priority: 2
SortPriority: 0
- Regex: '^(<|"(gtest|gmock|isl|json)/)'
Priority: 3
SortPriority: 0
- Regex: '.*'
Priority: 1
SortPriority: 0
IncludeIsMainRegex: '(Test)?$'
IncludeIsMainSourceRegex: ''
IndentCaseLabels: true
IndentGotoLabels: true
IndentPPDirectives: None
IndentWidth: 4
IndentWrappedFunctionNames: false
JavaScriptQuotes: Leave
JavaScriptWrapImports: true
KeepEmptyLinesAtTheStartOfBlocks: true
MacroBlockBegin: ''
MacroBlockEnd: ''
MaxEmptyLinesToKeep: 1
NamespaceIndentation: None
ObjCBinPackProtocolList: Auto
ObjCBlockIndentWidth: 4
ObjCSpaceAfterProperty: true
ObjCSpaceBeforeProtocolList: false
PenaltyBreakAssignment: 2
PenaltyBreakBeforeFirstCallParameter: 19
PenaltyBreakComment: 300
PenaltyBreakFirstLessLess: 120
PenaltyBreakString: 1000
PenaltyBreakTemplateDeclaration: 10
PenaltyExcessCharacter: 1000000
PenaltyReturnTypeOnItsOwnLine: 200
PointerAlignment: Right
ReflowComments: true
SortIncludes: false
SortUsingDeclarations: false
SpaceAfterCStyleCast: false
SpaceAfterLogicalNot: false
SpaceAfterTemplateKeyword: false
SpaceBeforeAssignmentOperators: true
SpaceBeforeCpp11BracedList: true
SpaceBeforeCtorInitializerColon: true
SpaceBeforeInheritanceColon: true
SpaceBeforeParens: ControlStatements
SpaceBeforeRangeBasedForLoopColon: true
SpaceInEmptyBlock: false
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 1
SpacesInAngles: false
SpacesInConditionalStatement: false
SpacesInContainerLiterals: false
SpacesInCStyleCastParentheses: false
SpacesInParentheses: false
SpacesInSquareBrackets: false
SpaceBeforeSquareBrackets: false
Standard: Latest
StatementMacros:
- Q_UNUSED
- QT_REQUIRE_VERSION
TabWidth: 8
UseCRLF: false
UseTab: Never
...

8
CMakeLists.txt
View File

@ -25,14 +25,6 @@ if (PICO_CYW43_SUPPORTED) # set by PICO_BOARD=pico_w
message("Using WIFI_PASSWORD from environment")
endif()
set(WIFI_SSID "${WIFI_SSID}" CACHE INTERNAL "WiFi SSID for examples")
set(WIFI_PASSWORD "${WIFI_PASSWORD}" CACHE INTERNAL "WiFi password for examples")
add_subdirectory(frtos)
if (NOT TARGET pico_btstack_base)
message("Skipping Pico W Bluetooth examples as support is not available")
else()
# add_subdirectory(bt)
endif()
endif()
endif()

6
frtos/CMakeLists.txt
View File

@ -1,6 +1,6 @@
include(FreeRTOS_Kernel_import.cmake)
add_subdirectory(wheel)
add_subdirectory(motor)
add_subdirectory(line_sensor)
add_executable(rtos_car rtos_car.c)
@ -13,8 +13,8 @@ target_compile_definitions(rtos_car PRIVATE
PING_USE_SOCKETS=1
)
target_include_directories(rtos_car PRIVATE
${CMAKE_CURRENT_LIST_DIR}
${CMAKE_CURRENT_LIST_DIR}/wheel
${CMAKE_CURRENT_LIST_DIR}/config
${CMAKE_CURRENT_LIST_DIR}/motor
${CMAKE_CURRENT_LIST_DIR}/line_sensor
)
target_link_libraries(rtos_car

0
frtos/FreeRTOSConfig.h → frtos/config/FreeRTOSConfig.h
View File

0
frtos/lwipopts.h → frtos/config/lwipopts.h
View File

2
frtos/lwipopts_examples_common.h → frtos/config/lwipopts_examples_common.h
View File

@ -7,7 +7,7 @@
// allow override in some examples
#ifndef NO_SYS
#define NO_SYS 1
#define NO_SYS 0
#endif
// allow override in some examples
#ifndef LWIP_SOCKET

34
frtos/config/motor_config.h Normal file
View File

@ -0,0 +1,34 @@
#ifndef MOTOR_CONFIG_H
#define MOTOR_CONFIG_H
// ENA and ENB on the L298N
#define PWM_PIN_LEFT 0U // chanel A
#define PWM_PIN_RIGHT 1U // chanel B
#define DIRECTION_PIN_RIGHT_IN1 11U
#define DIRECTION_PIN_RIGHT_IN2 12U
#define DIRECTION_PIN_LEFT_IN3 19U
#define DIRECTION_PIN_LEFT_IN4 20U
#define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2)
#define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1)
#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4)
#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3)
#define SPEED_PIN_RIGHT 15U
#define SPEED_PIN_LEFT 16U
#define PWM_CLK_DIV 250.f
#define PWM_WRAP 5000U
#define PID_KP 10.f
#define PID_KI 0.0f
#define PID_KD 0.0f
#define START_SPEED 4900U
#define MAX_SPEED 4900U
#define MIN_SPEED 0U // To be changed
#endif /* MOTOR_CONFIG_H */

16
frtos/motor/CMakeLists.txt Normal file
View File

@ -0,0 +1,16 @@
add_executable(motor_test motor_test.c)
target_link_libraries(motor_test
pico_cyw43_arch_lwip_sys_freertos
pico_stdlib
pico_lwip_iperf
FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
hardware_pwm
)
target_include_directories(motor_test PRIVATE
../config
)
pico_enable_stdio_usb(motor_test 1)
pico_add_extra_outputs(motor_test)

30
frtos/motor/motor_direction.h Normal file
View File

@ -0,0 +1,30 @@
/*
* @file motor_direction.c
* @brief control the direction of the wheels by setting the GPIO mask
* @author Richie
*/
#include "motor_init.h"
/*!
* @brief Set the direction of the wheels; can use bitwise OR to set both
* wheels such as DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_BACKWARD, it will
* set the left motor to go forward and the right motor to go backward within
* the same function.
* if the motor direction is not set, it will not move.
* @param direction The direction of the left and right wheels
* @param left_speed The speed of the left motor, from 0.0 to 1.0
* @param right_speed The speed of the right motor, from 0.0 to 1.0
*/
void
set_wheel_direction (uint32_t direction)
{
static const uint32_t mask = DIRECTION_LEFT_FORWARD |
DIRECTION_LEFT_BACKWARD |
DIRECTION_RIGHT_FORWARD |
DIRECTION_RIGHT_BACKWARD;
gpio_put_masked(mask, 0U);
gpio_set_mask(direction);
}

73
frtos/motor/motor_init.h Normal file
View File

@ -0,0 +1,73 @@
/*
* @file motor_init.h
* @brief define the constants and initialize the motor
* @author Richie
*/
#ifndef MOTOR_INIT_H
#define MOTOR_INIT_H
#include <stdio.h>
#include "pico/cyw43_arch.h"
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "motor_config.h"
uint g_slice_num_left = 0U;
uint g_slice_num_right = 0U;
SemaphoreHandle_t g_wheel_speed_sem_left = NULL;
SemaphoreHandle_t g_wheel_speed_sem_right = NULL;
void
motor_init(void)
{
// Semaphore
g_wheel_speed_sem_left = xSemaphoreCreateBinary();
g_wheel_speed_sem_right = xSemaphoreCreateBinary();
gpio_init(SPEED_PIN_RIGHT);
gpio_init(SPEED_PIN_LEFT);
gpio_set_dir(SPEED_PIN_RIGHT, GPIO_IN);
gpio_set_dir(SPEED_PIN_LEFT, GPIO_IN);
// Initialize direction pins as outputs
gpio_init(DIRECTION_PIN_RIGHT_IN1);
gpio_init(DIRECTION_PIN_RIGHT_IN2);
gpio_init(DIRECTION_PIN_LEFT_IN3);
gpio_init(DIRECTION_PIN_LEFT_IN4);
gpio_set_dir(DIRECTION_PIN_RIGHT_IN1, GPIO_OUT);
gpio_set_dir(DIRECTION_PIN_RIGHT_IN2, GPIO_OUT);
gpio_set_dir(DIRECTION_PIN_LEFT_IN3, GPIO_OUT);
gpio_set_dir(DIRECTION_PIN_LEFT_IN4, GPIO_OUT);
// Initialise PWM
gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM);
gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM);
g_slice_num_left = pwm_gpio_to_slice_num(PWM_PIN_LEFT);
g_slice_num_right = pwm_gpio_to_slice_num(PWM_PIN_RIGHT);
// NOTE: PWM clock is 125MHz for raspberrypi pico w by default
// 125MHz / 250 = 500kHz
pwm_set_clkdiv(g_slice_num_left, PWM_CLK_DIV);
pwm_set_clkdiv(g_slice_num_right, PWM_CLK_DIV);
// have them to be 500kHz / 5000 = 100Hz
pwm_set_wrap(g_slice_num_left, (PWM_WRAP - 1U));
pwm_set_wrap(g_slice_num_right, (PWM_WRAP - 1U));
pwm_set_enabled(g_slice_num_left, true);
pwm_set_enabled(g_slice_num_right, true);
}
#endif /* MOTOR_INIT_H */

196
frtos/motor/motor_speed.h Normal file
View File

@ -0,0 +1,196 @@
/*
* @file motor_speed.c
* @brief control the speed of the wheels by setting the PWM level, and
* monitor the speed by using edge interrupt and measure the time between
* each slot of the wheel, then calculate the speed of the wheel in cm/s, and
* adjust the speed of the wheel by using PID controller, and set the new PWM
* @author Richie
*/
#include "motor_init.h"
/*!
* @brief Set the speed of the wheels; can use bitwise OR to set both
* @param speed in range [0, 5000]
* @param side 0 for left, 1 for right
*/
void
set_wheel_speed(float speed, uint8_t side)
{
if (side == 0U)
{
pwm_set_chan_level(g_slice_num_left, PWM_CHAN_A, (uint16_t)speed);
}
else
{
pwm_set_chan_level(g_slice_num_right, PWM_CHAN_B, (uint16_t)speed);
}
}
void
h_left_wheel_sensor_isr_handler(void)
{
if (gpio_get_irq_event_mask(SPEED_PIN_LEFT) & GPIO_IRQ_EDGE_FALL)
{
gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
// printf("left motor sensor isr\n");
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_wheel_speed_sem_left,
&xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void
h_right_wheel_sensor_isr_handler(void)
{
if (gpio_get_irq_event_mask(SPEED_PIN_RIGHT) & GPIO_IRQ_EDGE_FALL)
{
gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL);
// printf("right motor sensor isr\n");
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_wheel_speed_sem_right,
&xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
float
compute_pid(float target_speed,
float current_speed,
float *integral,
float *prev_error)
{
float error = target_speed - current_speed;
*integral += error;
float derivative = error - *prev_error;
float control_signal
= PID_KP * error + PID_KI * (*integral) + PID_KD * derivative;
*prev_error = error;
return control_signal;
}
void
monitor_left_wheel_speed_task(void *pvParameters)
{
// static float * target_speed = NULL;
// *target_speed = * (float *) pvParameters;
for (;;)
{
if (xSemaphoreTake(g_wheel_speed_sem_left, portMAX_DELAY) == pdTRUE)
{
static uint64_t curr_time_left = 0u;
curr_time_left = time_us_64();
static uint64_t prev_time_left = 0u;
static uint64_t elapsed_time_left = 1u; // to avoid division by 0
elapsed_time_left = curr_time_left - prev_time_left;
prev_time_left = curr_time_left;
static float speed_left = 0.f;
// speed in cm/s; speed = distance / time
// distance = circumference / 20
// circumference = 2 * pi * 3.25 cm = 20.4203522483 cm
// distance = 20.4203522483 cm / 20 = 1.02101761242 cm
speed_left
= (float)(1.02101761242f / (elapsed_time_left / 1000000.f));
printf("left speed: %f cm/s\n", speed_left);
static float control_signal = 0.f;
static float integral = 0.f;
static float prev_error = 0.f;
control_signal = compute_pid(
*(float *)pvParameters, speed_left, &integral, &prev_error);
static float new_pwm = START_SPEED;
if (new_pwm + control_signal > MAX_SPEED)
{
new_pwm = MAX_SPEED;
}
else if (new_pwm + control_signal < MIN_SPEED)
{
new_pwm = MIN_SPEED;
}
else
{
new_pwm = new_pwm + control_signal;
}
printf("control signal: %f\n", control_signal);
printf("new pwm: %f\n\n", new_pwm);
set_wheel_speed(new_pwm, 0u);
}
}
}
void
monitor_right_wheel_speed_task(void *pvParameters)
{
// volatile float * target_speed = (float *) pvParameters;
// static volatile float * target_speed = NULL;
// target_speed = (float *) pvParameters;
for (;;)
{
if (xSemaphoreTake(g_wheel_speed_sem_right, portMAX_DELAY) == pdTRUE)
{
static uint64_t curr_time_right = 0u;
curr_time_right = time_us_64();
static uint64_t prev_time_right = 0u;
static uint64_t elapsed_time_right = 1u; // to avoid division by 0
elapsed_time_right = curr_time_right - prev_time_right;
prev_time_right = curr_time_right;
static float speed_right = 0.f;
speed_right
= (float)(1.02101761242f / (elapsed_time_right / 1000000.f));
printf("right speed: %f cm/s\n", speed_right);
static float control_signal = 0.f;
static float integral = 0.f;
static float prev_error = 0.f;
control_signal = compute_pid(
*(float *)pvParameters, speed_right, &integral, &prev_error);
static float new_pwm = START_SPEED;
if (new_pwm + control_signal > MAX_SPEED)
{
new_pwm = MAX_SPEED;
}
else if (new_pwm + control_signal < MIN_SPEED)
{
new_pwm = MIN_SPEED;
}
else
{
new_pwm = new_pwm + control_signal;
}
printf("control signal: %f\n", control_signal);
printf("new pwm: %f\n\n", new_pwm);
set_wheel_speed(new_pwm, 1u);
}
}
}

64
frtos/motor/motor_test.c Normal file
View File

@ -0,0 +1,64 @@
#include "motor_speed.h"
#include "motor_direction.h"
#define READ_LEFT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
#define READ_RIGHT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
void
launch()
{
// isr to detect right motor slot
gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true);
gpio_add_raw_irq_handler(SPEED_PIN_RIGHT,
h_right_wheel_sensor_isr_handler);
// isr to detect left motor slot
gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true);
gpio_add_raw_irq_handler(SPEED_PIN_LEFT,
h_left_wheel_sensor_isr_handler);
irq_set_enabled(IO_IRQ_BANK0, true);
static volatile float * p_target_speed = NULL;
static volatile float target_speed = 20.0f; // cm/s
p_target_speed = &target_speed;
TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_left_wheel_speed_task,
"monitor_left_wheel_speed_task",
configMINIMAL_STACK_SIZE,
(void *) p_target_speed,
READ_LEFT_WHEEL_SPEED_PRIO,
&h_monitor_left_wheel_speed_task_handle);
TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_right_wheel_speed_task,
"monitor_right_wheel_speed_task",
configMINIMAL_STACK_SIZE,
(void *) p_target_speed,
READ_RIGHT_WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle);
vTaskStartScheduler();
}
int
main (void)
{
stdio_usb_init();
sleep_ms(2000);
printf("Test started!\n");
motor_init();
set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD);
set_wheel_speed(START_SPEED, 0u);
launch();
// for(;;);
return (0);
}

27
frtos/rtos_car.c
View File

@ -7,18 +7,13 @@
// pico sdk libraries
#include "pico/cyw43_arch.h"
#include "pico/stdlib.h"
#include "hardware/adc.h"
#include "hardware/pwm.h"
// FreeRTOS libraries
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "message_buffer.h"
#include "semphr.h"
#include "wheel.h"
#include "line_sensor.h"
#include "motor_speed.h"
#include "motor_direction.h"
#define READ_LEFT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
#define READ_RIGHT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
@ -26,15 +21,12 @@
void
launch()
{
// isr to detect right wheel slot
// isr to detect right motor slot
gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true);
gpio_add_raw_irq_handler(SPEED_PIN_RIGHT,
h_right_wheel_sensor_isr_handler);
// isr to detect left wheel slot
// isr to detect left motor slot
gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true);
gpio_add_raw_irq_handler(SPEED_PIN_LEFT,
h_left_wheel_sensor_isr_handler);
@ -68,18 +60,13 @@ int
main (void)
{
stdio_usb_init();
wheel_setup();
sleep_ms(5000);
set_wheel_direction(DIRECTION_RIGHT_FORWARD);
set_wheel_speed(START_SPEED, 1u);
motor_init();
set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD);
launch();
while (1)
{
}
return (0);
}

1
frtos/wheel/CMakeLists.txt
View File

@ -1 +0,0 @@

269
frtos/wheel/wheel.h
View File

@ -1,269 +0,0 @@
// GPIO 2 as the PWM output, GPIO 26 as the ADC input
#define PWM_PIN_LEFT 0U // chanel A
#define PWM_PIN_RIGHT 1U // chanel B
#define DIRECTION_PIN_RIGHT_IN1 11U
#define DIRECTION_PIN_RIGHT_IN2 12U
#define DIRECTION_PIN_LEFT_IN3 19U
#define DIRECTION_PIN_LEFT_IN4 20U
#define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2)
#define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1)
#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4)
#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3)
#define SPEED_PIN_RIGHT 15U
#define SPEED_PIN_LEFT 16U
#define PWM_CLK_DIV 250.f
#define PWM_WRAP 5000U
#define PID_KP 10.f
#define PID_KI 0.0f
#define PID_KD 0.0f
#define START_SPEED 1500U
#define MAX_SPEED 4900U
#define MIN_SPEED 0U // To be changed
uint g_slice_num_left = 0U;
uint g_slice_num_right = 0U;
SemaphoreHandle_t g_wheel_speed_sem_left = NULL;
SemaphoreHandle_t g_wheel_speed_sem_right = NULL;
void
wheel_setup(void)
{
// Semaphore
g_wheel_speed_sem_left = xSemaphoreCreateBinary();
g_wheel_speed_sem_right = xSemaphoreCreateBinary();
gpio_init(SPEED_PIN_RIGHT);
gpio_init(SPEED_PIN_LEFT);
gpio_set_dir(SPEED_PIN_RIGHT, GPIO_IN);
gpio_set_dir(SPEED_PIN_LEFT, GPIO_IN);
// Initialize direction pins as outputs
gpio_init(DIRECTION_PIN_RIGHT_IN1);
gpio_init(DIRECTION_PIN_RIGHT_IN2);
gpio_init(DIRECTION_PIN_LEFT_IN3);
gpio_init(DIRECTION_PIN_LEFT_IN4);
gpio_set_dir(DIRECTION_PIN_RIGHT_IN1, GPIO_OUT);
gpio_set_dir(DIRECTION_PIN_RIGHT_IN2, GPIO_OUT);
gpio_set_dir(DIRECTION_PIN_LEFT_IN3, GPIO_OUT);
gpio_set_dir(DIRECTION_PIN_LEFT_IN4, GPIO_OUT);
// Initialise PWM
gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM);
gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM);
g_slice_num_left = pwm_gpio_to_slice_num(PWM_PIN_LEFT);
g_slice_num_right = pwm_gpio_to_slice_num(PWM_PIN_RIGHT);
// NOTE: PWM clock is 125MHz for raspberrypi pico w by default
// 125MHz / 250 = 500kHz
pwm_set_clkdiv(g_slice_num_left, PWM_CLK_DIV);
pwm_set_clkdiv(g_slice_num_right, PWM_CLK_DIV);
// have them to be 500kHz / 5000 = 100Hz
pwm_set_wrap(g_slice_num_left, (PWM_WRAP - 1U));
pwm_set_wrap(g_slice_num_right, (PWM_WRAP - 1U));
pwm_set_enabled(g_slice_num_left, true);
pwm_set_enabled(g_slice_num_right, true);
}
/*!
* @brief Set the direction of the wheels; can use bitwise OR to set both
* wheels such as DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_BACKWARD, it will
* set the left wheel to go forward and the right wheel to go backward within
* the same function.
* if the wheel direction is not set, it will not move.
* @param direction The direction of the left and right wheels
* @param left_speed The speed of the left wheel, from 0.0 to 1.0
* @param right_speed The speed of the right wheel, from 0.0 to 1.0
*/
void
set_wheel_direction (uint32_t direction)
{
static const uint32_t mask = DIRECTION_LEFT_FORWARD |
DIRECTION_LEFT_BACKWARD |
DIRECTION_RIGHT_FORWARD |
DIRECTION_RIGHT_BACKWARD;
gpio_put_masked(mask, 0U);
gpio_set_mask(direction);
}
/*!
* @brief Set the speed of the wheels; can use bitwise OR to set both
* @param speed in range [0, 5000]
* @param side 0 for left, 1 for right
*/
void
set_wheel_speed (float speed, uint8_t side)
{
if (side == 0U)
{
pwm_set_chan_level(g_slice_num_left,
PWM_CHAN_A,
(uint16_t) speed);
}
else
{
pwm_set_chan_level(g_slice_num_right,
PWM_CHAN_B,
(uint16_t) speed);
}
}
void
h_left_wheel_sensor_isr_handler (void)
{
if (gpio_get_irq_event_mask(SPEED_PIN_LEFT) & GPIO_IRQ_EDGE_FALL)
{
gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
// printf("left wheel sensor isr\n");
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_wheel_speed_sem_left, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void
h_right_wheel_sensor_isr_handler (void)
{
if (gpio_get_irq_event_mask(SPEED_PIN_RIGHT) & GPIO_IRQ_EDGE_FALL)
{
gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL);
// printf("right wheel sensor isr\n");
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_wheel_speed_sem_right, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
float
compute_pid(float target_speed, float current_speed, float * integral, float * prev_error)
{
float error = target_speed - current_speed;
*integral += error;
float derivative = error - *prev_error;
float control_signal = PID_KP * error +
PID_KI * (*integral) +
PID_KD * derivative;
*prev_error = error;
return control_signal;
}
void
monitor_left_wheel_speed_task (void *pvParameters)
{
static float * target_speed = NULL;
*target_speed = * (float *) pvParameters;
for (;;)
{
if (xSemaphoreTake(g_wheel_speed_sem_left, portMAX_DELAY) == pdTRUE)
{
static uint64_t curr_time_left = 0u;
curr_time_left = time_us_64();
static uint64_t prev_time_left = 0u;
static uint64_t elapsed_time_left = 1u; // to avoid division by 0
elapsed_time_left = curr_time_left - prev_time_left;
prev_time_left = curr_time_left;
static float speed_left = 0.f;
// speed in cm/s; speed = distance / time
// distance = circumference / 20
// circumference = 2 * pi * 3.25 cm = 20.4203522483 cm
// distance = 20.4203522483 cm / 20 = 1.02101761242 cm
speed_left = (float)
(1.02101761242f / (elapsed_time_left / 1000000.f));
printf("left speed: %f cm/s\n", speed_left);
}
}
}
void
monitor_right_wheel_speed_task (void *pvParameters)
{
// volatile float * target_speed = (float *) pvParameters;
static volatile float * target_speed = NULL;
target_speed = (float *) pvParameters;
for (;;)
{
if (xSemaphoreTake(g_wheel_speed_sem_right, portMAX_DELAY) == pdTRUE)
{
static uint64_t curr_time_right = 0u;
curr_time_right = time_us_64();
static uint64_t prev_time_right = 0u;
static uint64_t elapsed_time_right = 1u; // to avoid division by 0
elapsed_time_right = curr_time_right - prev_time_right;
prev_time_right = curr_time_right;
static float speed_right = 0.f;
speed_right = (float)
(1.02101761242f / (elapsed_time_right / 1000000.f));
printf("right speed: %f cm/s\n", speed_right);
static float control_signal = 0.f;
static float integral = 0.f;
static float prev_error = 0.f;
control_signal = compute_pid(*target_speed,
speed_right,
&integral,
&prev_error);
static float new_pwm = START_SPEED;
if (new_pwm + control_signal > MAX_SPEED)
{
new_pwm = MAX_SPEED;
}
else if (new_pwm + control_signal < MIN_SPEED)
{
new_pwm = MIN_SPEED;
}
else
{
new_pwm = new_pwm + control_signal;
}
printf("control signal: %f\n", control_signal);
printf("new pwm: %f\n\n", new_pwm);
set_wheel_speed(new_pwm, 1u);
}
}
}