Working version 1

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)

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);
+}
+

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

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

src/pwm.c

@@ -1,4 +1,6 @@
 #include "servo.h"
+#include "hardware/timer.h"
+#include "hardware/pwm.h"
 
 volatile uint16_t pwm_count;
 

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) {

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)