Finished Head Softaware

stone-dispencer-control/firmware-stone-dispencer/include/headSystem.hpp

@@ -0,0 +1,43 @@
+#pragma once
+#include "pinConfig.hpp"
+#include "FreeRTOS.h"
+#include "queue.h"
+#include "semphr.h"
+
+enum STONE_STATE {
+    EMPTY = 0b00, FULL = 0b10, LOW = 0b01
+};
+
+void vHeadTaskFn(void *pvParameters);
+
+class HeadSystem {
+private:
+    unsigned int motor_chopper1_slice_num;
+    unsigned int motor_chopper2_slice_num;
+    unsigned int motor_z_axis_slice_num;
+
+    bool headUp = false;
+
+    STONE_STATE stoneState;
+    xQueueHandle headStateQueue;
+    xSemaphoreHandle headSateSemaphore;
+    xTaskHandle headTaskHandle;
+
+public:
+    HeadSystem();
+
+    void initPins();
+
+    void setChopper(bool open1, bool open2);
+    void setHeadUp(bool up);
+
+    bool preLoad();
+    STONE_STATE dropStone();
+    
+    STONE_STATE dropSequence();
+
+    void vHeadTask();
+
+    
+};
+

stone-dispencer-control/firmware-stone-dispencer/include/pinConfig.hpp

@@ -0,0 +1,57 @@
+#pragma once
+#include "pico/stdlib.h"
+
+#define SENSOR_STONE_PIN 6 
+#define SENSOR_Z_AXIS_PIN 7
+
+#define MOTOR_CHOPPER1_PIN 11
+#define MOTOR_CHOPPER2_PIN 12
+#define MOTOR_Z_AXIS_PIN 13
+
+#define LED1_PIN 25
+#define LED2_PIN 16
+#define BUTTON_PIN 20
+
+#define GOBOTRPC_SCL_PIN 5
+#define GOBOTRPC_SDA_PIN 4
+#define GOBOTRPC_INT_PIN 2
+
+
+// Pwm Config
+
+#define PWM_TARGET_FREQ_HZ 100000.f
+#define PWM_CLK_DIV ((float)(SYS_CLK_HZ) / PWM_TARGET_FREQ_HZ)
+// 10us per Cycle -> 20ms / 10us = 2000
+#define PWM_TOTAL 11080
+
+#define PWM_CALC_CYCLE(n) ((int)((PWM_TOTAL/20.f) * (n)))
+
+#define MOTOR_MS18_MIN PWM_CALC_CYCLE(0.48f)
+#define MOTOR_MS18_MAX PWM_CALC_CYCLE(2.5f)
+#define MOTOR_MS18_MID (MOTOR_MS18_MAX + MOTOR_MS18_MIN) / 2
+
+#define MOTOR_MG996R_MIN PWM_CALC_CYCLE(0.36f)
+#define MOTOR_MG996R_MAX PWM_CALC_CYCLE(2.6f)
+#define MOTOR_MG966R_HALF_RANGE ((MOTOR_MG996R_MAX - MOTOR_MG996R_MIN) / 2)
+#define MOTOR_MG996R_MID (MOTOR_MG996R_MAX + MOTOR_MG996R_MIN) / 2
+
+#define PWM_INVERT(n) ((PWM_TOTAL + 1) - (n))
+
+// Chopper - Motor Mapping
+#define MOTOR_CHOPPER1 0
+#define MOTOR_CHOPPER2 1
+
+// Chopper States
+#define CHOPPER1_OPEN  MOTOR_MS18_MID - 400
+#define CHOPPER1_CLOSE MOTOR_MS18_MID + 40
+#define CHOPPER2_OPEN  MOTOR_MS18_MID + 375
+#define CHOPPER2_CLOSE MOTOR_MS18_MID + 30
+
+// Z Axis Config
+#define Z_AXIS_OFF  MOTOR_MS18_MID
+#define Z_AXIS_UP   MOTOR_MS18_MID + ((int)(MOTOR_MG966R_HALF_RANGE) * 0.9f)
+#define Z_AXIS_DOWN MOTOR_MS18_MID - ((int)(MOTOR_MG966R_HALF_RANGE) * 0.9f)
+#define Z_AXIS_DOWN_TIME 600
+#define Z_AXIS_UP_TIME 600
+
+#define Z_AXIS_IS_UP() (gpio_get(SENSOR_Z_AXIS_PIN) == 1)