alexander/gobot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged i2c-helper in Main

Browse Source
This commit is contained in:
AlexanderHD27
2025-01-12 00:16:00 +01:00
parent f4792de050
commit b5c7e5b4c1
396 changed files with 182143 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
stone-dispencer-control/firmware-stone-dispencer/src/headSystem/chopper.cpp Normal file
View File

@@ -0,0 +1,64 @@
#include "headSystem.hpp"
#include "hardware/pwm.h"
#include "FreeRTOS.h"
#include "task.h"
void HeadSystem::setChopper(bool open1, bool open2) {
unsigned setpoint1 = open1 ? CHOPPER1_OPEN : CHOPPER1_CLOSE;
unsigned setpoint2 = open2 ? CHOPPER2_OPEN : CHOPPER2_CLOSE;
setpoint1 = PWM_INVERT(setpoint1);
setpoint2 = PWM_INVERT(setpoint2);
if(MOTOR_CHOPPER1) {
pwm_set_gpio_level(MOTOR_CHOPPER2_PIN, setpoint1);
} else {
pwm_set_gpio_level(MOTOR_CHOPPER1_PIN, setpoint1);
}
if(MOTOR_CHOPPER2) {
pwm_set_gpio_level(MOTOR_CHOPPER2_PIN, setpoint2);
} else {
pwm_set_gpio_level(MOTOR_CHOPPER1_PIN, setpoint2);
}
}
STONE_STATE HeadSystem::dropStone() {
if(!gpio_get(SENSOR_STONE_PIN)) {
setChopper(true, false);
vTaskDelay(250 / portTICK_PERIOD_MS);
setChopper(false, false);
vTaskDelay(100 / portTICK_PERIOD_MS);
if(!gpio_get(SENSOR_STONE_PIN)) {
return EMPTY;
}
}
setChopper(false, true);
vTaskDelay(250 / portTICK_PERIOD_MS);
setChopper(false, false);
vTaskDelay(100 / portTICK_PERIOD_MS);
setChopper(true, false);
vTaskDelay(250 / portTICK_PERIOD_MS);
setChopper(false, false);
vTaskDelay(100 / portTICK_PERIOD_MS);
return gpio_get(SENSOR_STONE_PIN) ? FULL : LOW;
}
bool HeadSystem::preLoad() {
if(gpio_get(SENSOR_STONE_PIN)) {
return true;
}
setChopper(true, false);
vTaskDelay(250 / portTICK_PERIOD_MS);
setChopper(false, false);
vTaskDelay(100 / portTICK_PERIOD_MS);
return gpio_get(SENSOR_STONE_PIN);
}

63
stone-dispencer-control/firmware-stone-dispencer/src/headSystem/init.cpp Normal file
View File

@@ -0,0 +1,63 @@
#include "headSystem.hpp"
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "semphr.h"
HeadSystem::HeadSystem() {
initPins();
}
void vHeadTaskFn(void *pvParameters) {
HeadSystem * headSystem = (HeadSystem *) pvParameters;
headSystem->vHeadTask();
}
void HeadSystem::initPins() {
stoneState = EMPTY;
headStateQueue = xQueueCreate(3, sizeof(bool));
headSateSemaphore = xSemaphoreCreateBinary();
xTaskCreate(vHeadTaskFn, "Head Task", 1024, this, 1, &headTaskHandle);
gpio_init(SENSOR_STONE_PIN);
gpio_set_dir(SENSOR_STONE_PIN, GPIO_IN);
gpio_pull_up(SENSOR_STONE_PIN);
gpio_init(SENSOR_Z_AXIS_PIN);
gpio_set_dir(SENSOR_Z_AXIS_PIN, GPIO_IN);
gpio_pull_down(SENSOR_Z_AXIS_PIN);
gpio_init(LED1_PIN);
gpio_set_dir(LED1_PIN, GPIO_OUT);
gpio_init(LED2_PIN);
gpio_set_dir(LED2_PIN, GPIO_OUT);
gpio_set_function(MOTOR_CHOPPER1_PIN, GPIO_FUNC_PWM);
gpio_set_function(MOTOR_CHOPPER2_PIN, GPIO_FUNC_PWM);
gpio_set_function(MOTOR_Z_AXIS_PIN, GPIO_FUNC_PWM);
gpio_init(BUTTON_PIN);
gpio_set_dir(BUTTON_PIN, GPIO_IN);
gpio_pull_up(BUTTON_PIN);
motor_chopper1_slice_num = pwm_gpio_to_slice_num(MOTOR_CHOPPER1_PIN);
motor_chopper2_slice_num = pwm_gpio_to_slice_num(MOTOR_CHOPPER2_PIN);
motor_z_axis_slice_num = pwm_gpio_to_slice_num(MOTOR_Z_AXIS_PIN);
pwm_config config = pwm_get_default_config();
pwm_config_set_wrap(&config, PWM_TOTAL);
pwm_config_set_clkdiv(&config, PWM_CLK_DIV);
pwm_init(motor_chopper1_slice_num, &config, true);
pwm_init(motor_chopper2_slice_num, &config, true);
pwm_init(motor_z_axis_slice_num, &config, true);
pwm_set_gpio_level(MOTOR_CHOPPER1_PIN, PWM_INVERT(MOTOR_MS18_MID));
pwm_set_gpio_level(MOTOR_CHOPPER2_PIN, PWM_INVERT(MOTOR_MS18_MID));
pwm_set_gpio_level(MOTOR_Z_AXIS_PIN, PWM_INVERT(MOTOR_MS18_MID));
setChopper(false, false);
}

19
stone-dispencer-control/firmware-stone-dispencer/src/headSystem/stoneDispencer.cpp Normal file
View File

@@ -0,0 +1,19 @@
#include "headSystem.hpp"
#include "FreeRTOS.h"
#include "task.h"
STONE_STATE HeadSystem::dropSequence() {
gpio_put(LED1_PIN, 1);
if(!preLoad()) {
this->stoneState = EMPTY;
return this->stoneState;
}
bool prev_head_state = headUp;
setHeadUp(false);
STONE_STATE res = dropStone();
setHeadUp(prev_head_state);
this->stoneState = res;
gpio_put(LED2_PIN, 0);
return res;
}

50
stone-dispencer-control/firmware-stone-dispencer/src/headSystem/z-axis.cpp Normal file
View File

@@ -0,0 +1,50 @@
#include "headSystem.hpp"
#include "hardware/pwm.h"
#include "pico/stdlib.h"
#include <stdio.h>
#include "FreeRTOS.h"
void HeadSystem::vHeadTask() {
while (1) {
bool new_state;
xQueueReceive(headStateQueue, &new_state, portMAX_DELAY);
// Units in milliseconds
const int READ_DELAY = 50;
const int MAX_TIME = 7000;
if(headUp != new_state) {
if(new_state && !Z_AXIS_IS_UP()) { // Head Up
pwm_set_gpio_level(MOTOR_Z_AXIS_PIN, PWM_INVERT(Z_AXIS_UP));
vTaskDelay(100 / portTICK_PERIOD_MS);
for(int i=0; i<MAX_TIME / READ_DELAY; i++) {
if(Z_AXIS_IS_UP())
break;
vTaskDelay(READ_DELAY / portTICK_PERIOD_MS);
}
pwm_set_gpio_level(MOTOR_Z_AXIS_PIN, PWM_INVERT(Z_AXIS_OFF));
} else if(!new_state && Z_AXIS_IS_UP()) { // Head Down
pwm_set_gpio_level(MOTOR_Z_AXIS_PIN, PWM_INVERT(Z_AXIS_DOWN));
vTaskDelay(Z_AXIS_DOWN_TIME / portTICK_PERIOD_MS);
pwm_set_gpio_level(MOTOR_Z_AXIS_PIN, PWM_INVERT(Z_AXIS_OFF));
}
headUp = new_state;
}
xSemaphoreGive(headSateSemaphore);
}
}
void HeadSystem::setHeadUp(bool up) {
bool s = up;
xQueueSend(headStateQueue, &s, portMAX_DELAY);
xSemaphoreTake(headSateSemaphore, portMAX_DELAY);
}

123
stone-dispencer-control/firmware-stone-dispencer/src/main.cpp Normal file
View File

@@ -0,0 +1,123 @@
#include <stdio.h>
#include "pico/stdlib.h"
#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "task.h"
#include "hardware/pwm.h"
#include "headSystem.hpp"
#include "node_interface_hardware.hpp"
#include "node_interface.hpp"
#include "protocol.hpp"
struct AppData {
xQueueHandle txQueue;
xQueueHandle rxQueue;
HeadSystem * headsystem;
GobotRPC_NI * gobotrpc_ni;
};
AppData g_appData;
void onRxPackage(void * args, char *data, uint16_t len, GobotRPCTypes type, GobotRPCNumber number) {
AppData * appData = (AppData *) args;
char txBuffer[32];
switch (number) {
case GobotRPCNumber::GET_INFO: {
InfoData info = appData->gobotrpc_ni->getInfo();
txBuffer[0] = (info.addr >> 24) & 0xFF;
txBuffer[1] = (info.addr >> 16) & 0xFF;
txBuffer[2] = (info.addr >> 8) & 0xFF;
txBuffer[3] = info.addr & 0xFF;
txBuffer[4] = info.type;
g_appData.gobotrpc_ni->sendPackage(txBuffer, 5, GobotRPCTypes::RESPONSE, GobotRPCNumber::GET_INFO);
break;
}
case GobotRPCNumber::DROP_STONE: {
STONE_STATE state = appData->headsystem->dropStone();
txBuffer[0] = state;
g_appData.gobotrpc_ni->sendPackage(txBuffer, 1, GobotRPCTypes::RESPONSE, GobotRPCNumber::DROP_STONE);
break;
}
case GobotRPCNumber::MOVE_Z_AXIS: {
bool up = data[0] > 0;
appData->headsystem->setHeadUp(up);
g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::MOVE_Z_AXIS);
break;
}
case GobotRPCNumber::RESET: {
softwareReset();
break;
}
default: {
break;
}
}
}
void vMainRPCHardwareTask(void *pvParameters) {
AppData * appData = (AppData *) pvParameters;
GobotRPC_NI_Hardware_RP2040_I2C gobotrpc_ni_hardware(
appData->txQueue, appData->rxQueue, CORE_MASK_GOBOTRPC,
i2c0, I2C_ADDR, GORPC_SDA_PIN, GORPC_SCL_PIN, GORPC_INT_PIN
);
while (1) {
vTaskDelay(500 / portTICK_PERIOD_MS);
gpio_put(LED1_PIN, 1);
vTaskDelay(500 / portTICK_PERIOD_MS);
gpio_put(LED1_PIN, 0);
}
}
void vMainTask(void *pvParameters) {
HeadSystem * headSystem = (HeadSystem *) pvParameters;
bool buttonState = false;
bool buttonLastState = false;
headSystem->setHeadUp(true);
vTaskSuspend(NULL);
}
int main()
{
stdio_init_all();
HeadSystem headSystem;
g_appData.headsystem = &headSystem;
printf("HeadSystem initialized\n");
// Creating Queues
g_appData.txQueue = xQueueCreate(16, sizeof(GobotRPC_NI_Package_Transport));
g_appData.rxQueue = xQueueCreate(16, sizeof(GobotRPC_NI_Package_Transport));
// Setting GobotRPC Node Interface
InfoData info = {.addr=I2C_ADDR, .type=NODE_TYPE_HEAD};
GobotRPC_NI gobotrpc_ni(CORE_MASK_HEAD, g_appData.txQueue, g_appData.rxQueue, info);
gobotrpc_ni.registerOnPackageRxCallback(onRxPackage, &g_appData);
g_appData.gobotrpc_ni = &gobotrpc_ni;
// Sending Reset Notification
char txBuffer[8];
g_appData.gobotrpc_ni->sendPackage(txBuffer, 0, GobotRPCTypes::RESPONSE, GobotRPCNumber::RESET);
// Creating Tasks
xTaskCreate(vMainTask, "Main Head Task", 1024, &headSystem, 1, NULL);
xTaskCreate(vMainRPCHardwareTask, "Main GoboRPC Task", 2048, &g_appData, 1, NULL);
vTaskStartScheduler();
while (1) {
tight_loop_contents();
}
}