316 lines
9.5 KiB
C++
Executable File
316 lines
9.5 KiB
C++
Executable File
/*
|
|
* 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());
|
|
}
|
|
|
|
|
|
}
|