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

Finished Head Softaware

Browse Source
This commit is contained in:
AlexanderHD27
2025-01-02 19:10:59 +01:00
parent 0bb125fad9
commit 78a5148a1e
18 changed files with 1316 additions and 64 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);
}

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

@@ -0,0 +1,45 @@
#include <stdio.h>
#include "pico/stdlib.h"
#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "task.h"
#include "hardware/pwm.h"
#include "headSystem.hpp"
void vMainTask(void *pvParameters) {
HeadSystem * headSystem = (HeadSystem *) pvParameters;
bool buttonState = false;
bool buttonLastState = false;
headSystem->setHeadUp(true);
while (1) {
buttonState = !gpio_get(BUTTON_PIN);
if(buttonState) {
STONE_STATE s = headSystem->dropSequence();
printf("Stone dropped: %d\n", s);
}
buttonLastState = buttonState;
vTaskDelay(pdMS_TO_TICKS(100));
}
}
int main()
{
stdio_init_all();
HeadSystem headSystem;
printf("HeadSystem initialized\n");
xTaskCreate(vMainTask, "Button Task", 1024, &headSystem, 1, NULL);
vTaskStartScheduler();
while (1) {
tight_loop_contents();
}
}