Did stuff
This commit is contained in:
AlexanderHD27
315
stone-dispencer/main/hello_world_main.cpp
Executable file
315
stone-dispencer/main/hello_world_main.cpp
Executable file
@@ -0,0 +1,315 @@
|
||||
/*
|
||||
* SPDX-FileCopyrightText: 2010-2022 Espressif Systems (Shanghai) CO LTD
|
||||
*
|
||||
* SPDX-License-Identifier: CC0-1.0
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <inttypes.h>
|
||||
#include "sdkconfig.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
|
||||
#include "driver/gpio.h"
|
||||
#include "driver/ledc.h"
|
||||
#include "time.h"
|
||||
|
||||
#include "mcp2521.hpp"
|
||||
#include "mcp2521_hardware_handle.hpp"
|
||||
|
||||
#define MCP2521_PIN_INT GPIO_NUM_5
|
||||
#define MCP2521_PIN_SCLK GPIO_NUM_18
|
||||
#define MCP2521_PIN_MISO GPIO_NUM_26
|
||||
#define MCP2521_PIN_MOSI GPIO_NUM_23
|
||||
#define MCP2521_PIN_CS GPIO_NUM_25
|
||||
|
||||
#define SERVO_0_PIN GPIO_NUM_2
|
||||
#define SERVO_1_PIN GPIO_NUM_4
|
||||
#define SERVO_2_PIN GPIO_NUM_19
|
||||
#define SERVO_3_PIN GPIO_NUM_21
|
||||
|
||||
#define LED_PIN GPIO_NUM_22
|
||||
|
||||
#define DETECTOR_PIN GPIO_NUM_32
|
||||
#define Z_AXIS_DETECT_PIN GPIO_NUM_35
|
||||
#define BUTTON_PIN GPIO_NUM_33
|
||||
|
||||
#define MOTOR_PERIOD_US 20000
|
||||
#define MOTOR_MIN_PULSE_US 500
|
||||
#define MOTOR_MAX_PULSE_US 2500
|
||||
|
||||
#define MOTOR_MG996R_US 20000
|
||||
#define MOTOR_MG996R_MIN_PULSE_US 500
|
||||
#define MOTOR_MG996R_MAX_PULSE_US 2500
|
||||
|
||||
#define PWM_RESOLUTION 18
|
||||
|
||||
#define PWM_MAX_VALUE (1 << PWM_RESOLUTION)
|
||||
#define MOTOR_MAX_VALUE (MOTOR_MAX_PULSE_US * PWM_MAX_VALUE / MOTOR_PERIOD_US)
|
||||
#define MOTOR_MIN_VALUE (MOTOR_MIN_PULSE_US * PWM_MAX_VALUE / MOTOR_PERIOD_US)
|
||||
#define MOTOR_CENTER_VALUE ((MOTOR_MAX_VALUE + MOTOR_MIN_VALUE) / 2)
|
||||
|
||||
#define MOTOR_MG996R_MAX_VALUE (MOTOR_MG996R_MAX_PULSE_US * PWM_MAX_VALUE / MOTOR_MG996R_US)
|
||||
#define MOTOR_MG996R_MIN_VALUE (MOTOR_MG996R_MIN_PULSE_US * PWM_MAX_VALUE / MOTOR_MG996R_US)
|
||||
#define MOTOR_MG996R_CENTER_VALUE ((MOTOR_MG996R_MAX_VALUE + MOTOR_MG996R_MIN_VALUE) / 2)
|
||||
|
||||
#define MOTOR_RANGE (MOTOR_MAX_VALUE - MOTOR_MIN_VALUE)
|
||||
#define MOTOR_HALF_RANGE (MOTOR_RANGE / 2)
|
||||
|
||||
#define MOTOR_MG996R_RANGE (MOTOR_MG996R_MAX_VALUE - MOTOR_MG996R_MIN_VALUE)
|
||||
#define MOTOR_MG996R_HALF_RANGE (MOTOR_MG996R_RANGE / 2)
|
||||
|
||||
#define CHOPPER_0_CLOSED (MOTOR_CENTER_VALUE - 1000)
|
||||
#define CHOPPER_1_CLOSED (MOTOR_CENTER_VALUE + 900)
|
||||
#define CHOPPER_0_OPEN (MOTOR_CENTER_VALUE - MOTOR_HALF_RANGE*0.75)
|
||||
#define CHOPPER_1_OPEN (MOTOR_CENTER_VALUE + MOTOR_HALF_RANGE*0.75)
|
||||
|
||||
#define HEAD_MOVE_UP_TIME 250
|
||||
#define HEAD_MOVE_UP (MOTOR_MG996R_CENTER_VALUE + MOTOR_MG996R_HALF_RANGE*0.5)
|
||||
#define HEAD_MOVE_STOP (MOTOR_MG996R_CENTER_VALUE)
|
||||
#define HEAD_MOVE_DOWN_TIME 750
|
||||
#define HEAD_MOVE_DOWN (MOTOR_MG996R_CENTER_VALUE - MOTOR_MG996R_HALF_RANGE*0.5)
|
||||
|
||||
// Z-Axis Motor
|
||||
bool motor_stop_up_flag = false;
|
||||
SemaphoreHandle_t motor_stop_down_semaphore;
|
||||
SemaphoreHandle_t motor_stop_up_semaphore;
|
||||
|
||||
static void IRAM_ATTR isr_motor_stop_up(void *args) {
|
||||
if(motor_stop_up_flag) {
|
||||
motor_stop_up_flag = false;
|
||||
xSemaphoreGiveFromISR(motor_stop_up_semaphore, NULL);
|
||||
}
|
||||
}
|
||||
|
||||
static void task_fn_motor_stop_up(void *args) {
|
||||
while(true) {
|
||||
xSemaphoreTake(motor_stop_up_semaphore, portMAX_DELAY);
|
||||
vTaskDelay(HEAD_MOVE_UP_TIME / portTICK_PERIOD_MS);
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2, HEAD_MOVE_STOP);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2);
|
||||
}
|
||||
}
|
||||
|
||||
static void task_fn_motor_stop_down(void *args) {
|
||||
while(true) {
|
||||
xSemaphoreTake(motor_stop_down_semaphore, portMAX_DELAY);
|
||||
vTaskDelay(HEAD_MOVE_DOWN_TIME / portTICK_PERIOD_MS);
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2, HEAD_MOVE_STOP);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2);
|
||||
}
|
||||
}
|
||||
|
||||
void lower_head() {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2, HEAD_MOVE_DOWN);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2);
|
||||
xSemaphoreGive(motor_stop_down_semaphore);
|
||||
vTaskDelay(500 / portTICK_PERIOD_MS);
|
||||
}
|
||||
|
||||
void raise_head() {
|
||||
if(!gpio_get_level(Z_AXIS_DETECT_PIN)) {
|
||||
return;
|
||||
}
|
||||
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2, HEAD_MOVE_UP);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2);
|
||||
motor_stop_up_flag = true;
|
||||
}
|
||||
|
||||
// Dispencing mechanism
|
||||
void set_copper(bool bottom, bool top) {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1, bottom ? CHOPPER_1_OPEN : CHOPPER_1_CLOSED);
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, top ? CHOPPER_0_OPEN : CHOPPER_0_CLOSED);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);
|
||||
}
|
||||
|
||||
enum DISPENSER_RESULT {
|
||||
SUCESS = 0,
|
||||
EMPTY_WARINING = 1,
|
||||
EMPTY = 2,
|
||||
};
|
||||
|
||||
bool load_stone() {
|
||||
if(!gpio_get_level(DETECTOR_PIN)) {
|
||||
// No stone loaded
|
||||
return true;
|
||||
}
|
||||
|
||||
set_copper(true, false);
|
||||
vTaskDelay(250 / portTICK_PERIOD_MS);
|
||||
set_copper(false, false);
|
||||
|
||||
if(!gpio_get_level(DETECTOR_PIN)) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
enum DISPENSER_RESULT dispens_stone() {
|
||||
|
||||
if(!gpio_get_level(DETECTOR_PIN)) {
|
||||
// No stone loaded
|
||||
|
||||
set_copper(false, true);
|
||||
vTaskDelay(250 / portTICK_PERIOD_MS);
|
||||
set_copper(false, false);
|
||||
vTaskDelay(100 / portTICK_PERIOD_MS);
|
||||
|
||||
if(!gpio_get_level(DETECTOR_PIN)) {
|
||||
// No stone loaded again
|
||||
return EMPTY;
|
||||
}
|
||||
|
||||
}
|
||||
lower_head();
|
||||
set_copper(true, false);
|
||||
vTaskDelay(250 / portTICK_PERIOD_MS);
|
||||
set_copper(false, false);
|
||||
vTaskDelay(100 / portTICK_PERIOD_MS);
|
||||
set_copper(false, true);
|
||||
vTaskDelay(250 / portTICK_PERIOD_MS);
|
||||
set_copper(false, false);
|
||||
raise_head();
|
||||
|
||||
if(!gpio_get_level(DETECTOR_PIN)) {
|
||||
return SUCESS;
|
||||
} else {
|
||||
return EMPTY_WARINING;
|
||||
}
|
||||
}
|
||||
|
||||
extern "C" void app_main(void)
|
||||
{
|
||||
printf("Hello world!\n");
|
||||
|
||||
gpio_set_direction(LED_PIN, GPIO_MODE_OUTPUT);
|
||||
gpio_set_direction(DETECTOR_PIN, GPIO_MODE_INPUT);
|
||||
gpio_set_direction(Z_AXIS_DETECT_PIN, GPIO_MODE_INPUT);
|
||||
gpio_set_direction(BUTTON_PIN, GPIO_MODE_INPUT);
|
||||
|
||||
motor_stop_up_semaphore = xSemaphoreCreateBinary();
|
||||
motor_stop_down_semaphore = xSemaphoreCreateBinary();
|
||||
xTaskCreatePinnedToCore(task_fn_motor_stop_up, "motor_stop_up", 2048, NULL, 4, NULL, 0);
|
||||
xTaskCreatePinnedToCore(task_fn_motor_stop_down, "motor_stop_down", 2048, NULL, 4, NULL, 0);
|
||||
|
||||
gpio_set_intr_type(Z_AXIS_DETECT_PIN, GPIO_INTR_ANYEDGE);
|
||||
gpio_install_isr_service(0);
|
||||
gpio_isr_handler_add(Z_AXIS_DETECT_PIN, isr_motor_stop_up, NULL);
|
||||
|
||||
spi_bus_config_t bus_config;
|
||||
MCP2521_HardwareHandle_ESP MCP2521_HardwareHandle_ESP(
|
||||
VSPI_HOST,
|
||||
&bus_config,
|
||||
MCP2521_PIN_MOSI,
|
||||
MCP2521_PIN_MISO,
|
||||
MCP2521_PIN_SCLK,
|
||||
MCP2521_PIN_CS,
|
||||
MCP2521_PIN_INT
|
||||
);
|
||||
|
||||
if(0) {
|
||||
gpio_set_direction(MCP2521_PIN_SCLK, GPIO_MODE_OUTPUT);
|
||||
gpio_set_direction(MCP2521_PIN_MISO, GPIO_MODE_OUTPUT);
|
||||
gpio_set_direction(MCP2521_PIN_MOSI, GPIO_MODE_OUTPUT);
|
||||
gpio_set_direction(MCP2521_PIN_CS, GPIO_MODE_OUTPUT);
|
||||
gpio_set_level(MCP2521_PIN_SCLK, 1);
|
||||
gpio_set_level(MCP2521_PIN_MISO, 1);
|
||||
gpio_set_level(MCP2521_PIN_MOSI, 1);
|
||||
gpio_set_level(MCP2521_PIN_CS, 1);
|
||||
vTaskDelay(50 / portTICK_PERIOD_MS);
|
||||
gpio_set_level(MCP2521_PIN_SCLK, 0);
|
||||
vTaskDelay(50 / portTICK_PERIOD_MS);
|
||||
gpio_set_level(MCP2521_PIN_MOSI, 0);
|
||||
vTaskDelay(50 / portTICK_PERIOD_MS);
|
||||
gpio_set_level(MCP2521_PIN_MISO, 0);
|
||||
vTaskDelay(50 / portTICK_PERIOD_MS);
|
||||
gpio_set_level(MCP2521_PIN_CS, 0); // Works
|
||||
}
|
||||
|
||||
MCP2521 mcp2521(&MCP2521_HardwareHandle_ESP);
|
||||
mcp2521.reset();
|
||||
mcp2521.set_mode_of_operation(MCP2521_OPERATION_MODE::NORMAL, true);
|
||||
printf("CANSTAT: %x\n", mcp2521.read_reg(MCP2521_CANCTRL));
|
||||
printf("MCP2521 initialized\n");
|
||||
|
||||
ledc_timer_config_t ledc_timer = {
|
||||
.speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
|
||||
.duty_resolution = LEDC_TIMER_18_BIT,
|
||||
.timer_num = LEDC_TIMER_0,
|
||||
.freq_hz = 50,
|
||||
.clk_cfg = LEDC_AUTO_CLK
|
||||
};
|
||||
|
||||
ledc_channel_config_t servo0_channel_config = {
|
||||
.gpio_num = SERVO_0_PIN,
|
||||
.speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
|
||||
.channel = LEDC_CHANNEL_0,
|
||||
.intr_type = LEDC_INTR_DISABLE,
|
||||
.timer_sel = LEDC_TIMER_0,
|
||||
.duty = 0,
|
||||
.hpoint = 0,
|
||||
.flags = {
|
||||
.output_invert = 1
|
||||
}
|
||||
};
|
||||
|
||||
ledc_channel_config_t servo1_channel_config = {
|
||||
.gpio_num = SERVO_1_PIN,
|
||||
.speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
|
||||
.channel = LEDC_CHANNEL_1,
|
||||
.intr_type = LEDC_INTR_DISABLE,
|
||||
.timer_sel = LEDC_TIMER_0,
|
||||
.duty = 0,
|
||||
.hpoint = 0,
|
||||
.flags = {
|
||||
.output_invert = 1
|
||||
}
|
||||
};
|
||||
|
||||
ledc_channel_config_t servo2_channel_config = {
|
||||
.gpio_num = SERVO_2_PIN,
|
||||
.speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
|
||||
.channel = LEDC_CHANNEL_2,
|
||||
.intr_type = LEDC_INTR_DISABLE,
|
||||
.timer_sel = LEDC_TIMER_0,
|
||||
.duty = 0,
|
||||
.hpoint = 0,
|
||||
.flags = {
|
||||
.output_invert = 1
|
||||
}
|
||||
};
|
||||
|
||||
ledc_channel_config_t servo3_channel_config = {
|
||||
.gpio_num = SERVO_3_PIN,
|
||||
.speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
|
||||
.channel = LEDC_CHANNEL_3,
|
||||
.intr_type = LEDC_INTR_DISABLE,
|
||||
.timer_sel = LEDC_TIMER_0,
|
||||
.duty = 0,
|
||||
.hpoint = 0,
|
||||
.flags = {
|
||||
.output_invert = 1
|
||||
}
|
||||
};
|
||||
|
||||
ledc_timer_config(&ledc_timer);
|
||||
ledc_channel_config(&servo0_channel_config);
|
||||
ledc_channel_config(&servo1_channel_config);
|
||||
ledc_channel_config(&servo2_channel_config);
|
||||
ledc_channel_config(&servo3_channel_config);
|
||||
|
||||
raise_head();
|
||||
set_copper(false, false);
|
||||
|
||||
while (true) {
|
||||
while (!gpio_get_level(BUTTON_PIN)) {
|
||||
vTaskDelay(50 / portTICK_PERIOD_MS);
|
||||
}
|
||||
printf("Sucess: %d\n", dispens_stone());
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user