Working version 1

elmot · Apr 23, 2023 · 6c4640a · 6c4640a
1 parent 5110e28
commit 6c4640a
Show file tree

Hide file tree

Showing 7 changed files with 129 additions and 81 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -13,7 +13,7 @@ set(PICO_BOARD pico)
 
 pico_sdk_init()
 
-add_executable(rpi-pico-servo src/servo.c src/pwm.c)
+add_executable(rpi-pico-servo src/servo.c src/pwm.c src/as560x.c)
 pico_set_program_name(rpi-pico-servo "rpi-pico-servo")
 pico_set_program_version(rpi-pico-servo "0.1")
 pico_add_extra_outputs(rpi-pico-servo)

diff --git a/src/as560x.c b/src/as560x.c
@@ -0,0 +1,55 @@
+#include "hardware/i2c.h"
+#include <machine/endian.h>
+
+#include "servo.h"
+#include "as560x.h"
+
+#define AS560x_ADDR 0x36
+#define AS560x_STATUS_REG (0x0B)
+#define AS560x_RAW_ANGLE_REG (0x0C)
+
+uint16_t as560xReadReg(int addr, bool wide, uint16_t mask) {
+    uint16_t buf;
+    int result = i2c_write_timeout_us(I2C, AS560x_ADDR, (uint8_t *) &addr, 1, true, I2C_TIMEOUT_US);
+    if (result <= 0) {
+        i2cError();
+    }
+    result = i2c_read_timeout_us(I2C, AS560x_ADDR, (uint8_t *) &buf, (wide ? 2 : 1), false, I2C_TIMEOUT_US);
+    if (result <= 0) {
+        i2cError();
+    }
+    if (wide) {
+        return __bswap16(buf) & mask;
+    } else {
+        return buf & mask;
+    }
+}
+
+void AS560x_print_reg16(const char *formatStr, int addr, uint16_t mask) {
+    uint16_t result = as560xReadReg(addr, true, mask);
+    printf(formatStr, result & mask);
+}
+
+void AS560x_print_reg8(const char *formatStr, int addr, uint8_t mask) {
+    uint8_t result = (uint8_t) as560xReadReg(addr, false, mask);
+    printf(formatStr, result & mask);
+}
+
+void as560x_init() {
+    i2c_init(i2c1, 100 * 1000);
+}
+
+int as560xReadAngle() {
+    return as560xReadReg(AS560x_RAW_ANGLE_REG, true, 0xFFF);
+}
+
+uint8_t as560xGetStatus() {
+    return (uint8_t) as560xReadReg(AS560x_STATUS_REG, false, 0x38);
+}
+
+__unused void sensorData() {
+    AS560x_print_reg8("Status: %02x; ", 0xb, 0x38);
+    AS560x_print_reg8("AGC: %3x; ", 0x1a, 0xff);
+    AS560x_print_reg16("Angle: %04x\n\r", 0x0c, 0xFFF);
+}
+
diff --git a/src/as560x.h b/src/as560x.h
@@ -0,0 +1,33 @@
+
+#ifndef RPI_PICO_SERVO_AS560X_H
+#define RPI_PICO_SERVO_AS560X_H
+#define I2C_TIMEOUT_US (100000)
+
+#define AS5601_ANGLE_MAX (0xFFFL)
+#define AS560x_STATUS_MAGNET_DETECTED (0x20)
+#define AS560x_STATUS_MAGNET_HIGH (0x08)
+#define AS560x_STATUS_MAGNET_LOW (0x10)
+
+/** Initializes sensor (i2c) bus
+ */
+void as560x_init();
+
+_Noreturn void i2cError();
+
+/** Returns raw angle from the sensor
+ *
+ * @return Raw angle value [0...AS5601_ANGLE_MAX]
+ */
+int as560xReadAngle();
+
+/** Reads sensor status
+ *
+ * @return combo of AS560x_STATUS_MAGNET_DETECTED, AS560x_STATUS_MAGNET_HIGH, AS560x_STATUS_MAGNET_LOW
+ */
+uint8_t as560xGetStatus();
+
+/** Debug purposes only
+ */
+__unused void sensorData();
+
+#endif //RPI_PICO_SERVO_AS560X_H
diff --git a/src/params.h b/src/params.h
@@ -0,0 +1,16 @@
+//
+// Created by elmot on 23 Apr 2023.
+//
+
+#ifndef RPI_PICO_SERVO_PARAMS_H
+#define RPI_PICO_SERVO_PARAMS_H
+//degrees
+#define ANGLE_TOLERANCE  (2)
+#define DEAD_ANGLE  (5)
+#define SLOW_ANGLE  (40)
+
+#define NO_PWM  (100)
+#define SLOW_PWM  (70)
+#define FAST_PWM  (20)
+
+#endif //RPI_PICO_SERVO_PARAMS_H
diff --git a/src/pwm.c b/src/pwm.c
@@ -1,4 +1,6 @@
 #include "servo.h"
+#include "hardware/timer.h"
+#include "hardware/pwm.h"
 
 volatile uint16_t pwm_count;
 

diff --git a/src/servo.c b/src/servo.c
@@ -1,114 +1,60 @@
 #include "servo.h"
-
-#define I2C_TIMEOUT_US (100000)
-
-#define AS5601_ADDR 0x36
-#define AS5601_STATUS_REG (0x0B)
-#define AS5601_RAW_ANGLE_REG (0x0C)
-#define AS5601_ANGLE_MAX (0xFFFL)
-#define AS5601_STATUS_MAGNET_DETECTED (0x20)
-#define AS5601_STATUS_MAGNET_HIGH (0x08)
-#define AS5601_STATUS_MAGNET_LOW (0x10)
-
-//degrees
-#define ANGLE_TOLERANCE  (35)
-#define DEAD_ANGLE  (5)
-#define SLOW_ANGLE  (30)
-
-#define SLOW_PWM  (60)
-#define FAST_PWM  (75)
-
-_Noreturn void i2cError();
+#include "params.h"
+#include "as560x.h"
 
 _Noreturn void magnetError();
 
-uint16_t as5601ReadReg(int addr, bool wide, uint16_t mask);
-
-/** Debug purposes only
- */
-__unused void sensorLoop();
-
 #pragma clang diagnostic push
 #pragma ide diagnostic ignored "EndlessLoop"
-int target_angle = 270;//todo
 
 int main() {
     stdio_init_all();
     gpio_init(LED_PIN);
     gpio_set_dir(LED_PIN, GPIO_OUT);
     init_pwm();
 
-    i2c_init(i2c1, 100 * 1000);
+    as560x_init();
     gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
     gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
     gpio_pull_up(I2C_SDA_PIN);
     gpio_pull_up(I2C_SCL_PIN);
 
     bi_decl(bi_2pins_with_func(I2C_SDA_PIN, I2C_SCL_PIN, GPIO_FUNC_I2C))
-
-    while(1) {
-        printf("PWM_IN: %d\n\r", pwm_count);
-    }
+    bool moving = true;
     while (1) {
-        uint8_t status = (uint8_t) as5601ReadReg(AS5601_STATUS_REG, false, 0x38);
-        if (!(status & AS5601_STATUS_MAGNET_DETECTED)) {
+        uint8_t status = (uint8_t) as560xGetStatus();
+        if (!(status & AS560x_STATUS_MAGNET_DETECTED)) {
             printf("ERROR\n\r");
             magnetError();
         }
+        if(pwm_count == 0) continue;
+        else if(pwm_count < 1000) pwm_count = 1000;
+        else if(pwm_count > 2000) pwm_count = 2000;
+        int target_angle = 3 + ((pwm_count - 1000) * 354) / 1000;
         //todo check getting stuck
-        int angle = as5601ReadReg(AS5601_RAW_ANGLE_REG, true, 0xFFF) * 360L / AS5601_ANGLE_MAX;
-        int angle_delta = (360 + target_angle - angle) % 360;
-        if (angle_delta > 180) { angle_delta = angle_delta - 360; }
+        int angle = as560xReadAngle() * 360L / AS5601_ANGLE_MAX;
+
+        int angle_delta = target_angle - angle;
         int angle_delta_abs = abs(angle_delta);
-        unsigned int pwm_a = 0, pwm_b = 0;
-        if (angle_delta_abs > ANGLE_TOLERANCE) {
+        int angle_tolerance = moving ? ANGLE_TOLERANCE : DEAD_ANGLE;
+        if (angle_delta_abs > angle_tolerance) {
             gpio_put(LED_PIN, 1);
-            int pwm = angle_delta_abs > SLOW_ANGLE ? FAST_PWM : SLOW_PWM;
+            moving = true;
+            int pwm = (angle_delta_abs > SLOW_ANGLE) ? FAST_PWM : SLOW_PWM;
             if (angle_delta > 0) {
-                pwm_a = pwm;
+                setMotorPwm(pwm, NO_PWM);
             } else {
-                pwm_b = pwm;
+                setMotorPwm(NO_PWM, pwm);
             }
         } else {
             gpio_put(LED_PIN, 0);
+            setMotorPwm(NO_PWM, NO_PWM);
+            moving = false;
+            printf("Target angle: %d; Current angle: %d\n\r",target_angle,angle);
         }
-        setMotorPwm(pwm_a, pwm_b);
     }
 }
 
-uint16_t as5601ReadReg(int addr, bool wide, uint16_t mask) {
-    uint16_t buf;
-    int result = i2c_write_timeout_us(I2C, AS5601_ADDR, (uint8_t *) &addr, 1, true, I2C_TIMEOUT_US);
-    if (result <= 0) {
-        i2cError();
-    }
-    result = i2c_read_timeout_us(I2C, AS5601_ADDR, (uint8_t *) &buf, (wide ? 2 : 1), false, I2C_TIMEOUT_US);
-    if (result <= 0) {
-        i2cError();
-    }
-    if (wide) {
-        return __bswap16(buf) & mask;
-    } else {
-        return buf & mask;
-    }
-}
-
-void AS5601_print_reg16(const char *formatStr, int addr, uint16_t mask) {
-    uint16_t result = as5601ReadReg(addr, true, mask);
-    printf(formatStr, result & mask);
-}
-
-void AS5601_print_reg8(const char *formatStr, int addr, uint8_t mask) {
-    uint8_t result = (uint8_t) as5601ReadReg(addr, false, mask);
-    printf(formatStr, result & mask);
-}
-
-__unused void sensorLoop() {
-    AS5601_print_reg8("Status: %02x; ", 0xb, 0x38);
-    AS5601_print_reg8("AGC: %3x; ", 0x1a, 0xff);
-    AS5601_print_reg16("Angle: %04x\n\r", 0x0c, 0xFFF);
-}
-
 _Noreturn static inline void error(int phase_on_ms, int phase_off_ms) {
     watchdog_enable(500, 1);
     while (1) {

diff --git a/src/servo.h b/src/servo.h
@@ -1,17 +1,13 @@
 #ifndef RPI_PICO_SERVO_SERVO_H
 #define RPI_PICO_SERVO_SERVO_H
-#include <machine/endian.h>
 #include <pico/assert.h>
 #include <pico/printf.h>
 #include <hardware/watchdog.h>
 #include <stdlib.h>
 #include "pico/stdlib.h"
 #include "pico/binary_info.h"
-#include "hardware/pwm.h"
-#include "hardware/i2c.h"
 #include "hardware/structs/clocks.h"
 #include "hardware/clocks.h"
-#include "hardware/timer.h"
 
 #define PWM_IN_PIN (21)