Added Drive and Arm Classes for SAR

It's time.

library/L3_Application/Robotics/Arm/ArmController.hpp

@@ -0,0 +1,57 @@
+#pragma once
+
+#include "utility/units.hpp"
+#include "L3_Application/Robotics/Common/ChiHaiServo.hpp"
+#include "L3_Application/Robotics/Arm/RotundaServo.hpp"
+
+namespace sjsu
+{
+namespace robotics
+{
+class ArmController
+{
+ public:
+  explicit constexpr ArmController(sjsu::robotics::ChiHaiServo & shoulder,
+                                   sjsu::robotics::ChiHaiServo & wrist,
+                                   sjsu::robotics::RotundaServo & rotunda)
+      : shoulder_(shoulder), wrist_(wrist), rotunda_(rotunda)
+  {
+  }
+
+  void Initialize()
+  {
+    shoulder_.Initialize();
+    wrist_.Initialize();
+    rotunda_.Initialize();
+  }
+
+  void SetShoulderAngle(units::angle::degree_t angle)
+  {
+    shoulder_.SetAngle(angle);
+  }
+
+  void SetWristAngle(units::angle::degree_t angle)
+  {
+    wrist_.SetAngle(angle);
+  }
+
+  void SetRotundaAngle(units::angle::degree_t angle)
+  {
+    rotunda_.SetAngle(angle);
+  }
+
+  void Stop() {
+    shoulder_.Stop();
+    wrist_.Stop();
+  }
+
+ private:
+  sjsu::robotics::ChiHaiServo & shoulder_;
+  sjsu::robotics::ChiHaiServo & wrist_;
+  sjsu::robotics::RotundaServo & rotunda_;
+  /* data */
+};
+
+}  // namespace robotics
+}  // namespace sjsu
+// ChiHaiServo.hpp World

library/L3_Application/Robotics/Arm/RTOSTasks.hpp

@@ -0,0 +1,31 @@
+#include <cstdint>
+#include "FreeRTOS.h"
+
+#include "L1_Peripheral/lpc40xx/i2c.hpp"
+#include "L1_Peripheral/lpc40xx/pwm.hpp"
+#include "L1_Peripheral/lpc40xx/gpio.hpp"
+#include "utility/log.hpp"
+#include "utility/rtos.hpp"
+#include "L3_Application/Robotics/Common/Params.hpp"
+#include "L3_Application/Robotics/Drive/HubMotor.hpp"
+
+// this is copied and pasted from drive rtos tasks. modify on thursday (2/20)
+void vMoveTask(void * pvParameters)
+{
+  sjsu::robotics::ParamsStruct * params = (sjsu::robotics::ParamsStruct *)pvParameters;
+  sjsu::lpc40xx::Gpio direction_a(0, 1);
+  sjsu::lpc40xx::Gpio direction_b(0, 2);
+  sjsu::lpc40xx::Gpio direction_c(0, 3);
+  sjsu::lpc40xx::Pwm pwm_a(sjsu::lpc40xx::Pwm::Channel::kPwm0);
+  sjsu::lpc40xx::Pwm pwm_b(sjsu::lpc40xx::Pwm::Channel::kPwm1);
+  sjsu::lpc40xx::Pwm pwm_c(sjsu::lpc40xx::Pwm::Channel::kPwm2);
+  sjsu::robotics::HubMotor wheel_a(pwm_a, direction_a);
+  sjsu::robotics::HubMotor wheel_b(pwm_b, direction_b);
+  sjsu::robotics::HubMotor wheel_c(pwm_c, direction_c);
+
+  while (1)
+  {
+    LOG_INFO("wheel_a: %d, arm_angle: %f", params->wheel_a, params->arm_angle);
+    vTaskDelay(100);
+  }
+}

library/L3_Application/Robotics/Arm/RotundaServo.hpp

@@ -0,0 +1,21 @@
+#pragma once
+#include "L2_HAL/actuators/servo/servo.hpp"
+
+namespace sjsu
+{
+namespace robotics
+{
+class RotundaServo : public Servo
+{
+ public:
+  explicit constexpr RotundaServo(const Pwm & pwm) : Servo(pwm) {}
+
+  void Initialize(units::frequency::hertz_t frequency = 50_Hz) override
+  {
+    Servo::Initialize(frequency);
+    Servo::SetAngleBounds(-1540_deg, 1540_deg);
+    Servo::SetPulseBounds(500us, 2500us);
+  }
+};
+}  // namespace robotics
+}  // namespace sjsu

library/L3_Application/Robotics/Common/ChiHaiServo.hpp

@@ -0,0 +1,49 @@
+#pragma once
+
+#include "L3_Application/Robotics/Common/MagneticEncoder.hpp"
+#include "L3_Application/Robotics/Common/Drv8801.hpp"
+#include "utility/units.hpp"
+
+namespace sjsu
+{
+namespace robotics
+{
+class ChiHaiServo
+{
+ public:
+  explicit constexpr ChiHaiServo(
+      sjsu::robotics::MagneticEncoder magnetic_encoder,
+      sjsu::robotics::Drv8801 drv)
+      : magnetic_encoder_(magnetic_encoder), drv_(drv)
+  {
+  }
+  void Initialize()
+  {
+    magnetic_encoder_.Initialize();
+    drv_.Initialize();
+  }
+  void SetAngle(units::angle::degree_t angle)
+  {
+    units::angle::degree_t current_degree = magnetic_encoder_.GetAngle();
+    // do some math to figure out whether we should turn clockwise or counter
+    // clockwise to get desired angle faster
+    if (current_degree - angle > 0_deg)
+    {
+      // haha do math here to see if we should go forward or back
+    }
+    // can i use something else than a while loop here?
+    while (magnetic_encoder_.GetAngle() != angle)
+    {
+    }
+  }
+  void Stop()
+  {
+    drv_.Stop();
+  }
+
+ private:
+  sjsu::robotics::MagneticEncoder magnetic_encoder_;
+  sjsu::robotics::Drv8801 drv_;
+};
+}  // namespace robotics
+}  // namespace sjsu

library/L3_Application/Robotics/Common/Drv8801.hpp

@@ -0,0 +1,59 @@
+#pragma once
+
+#include "utility/log.hpp"
+#include "utility/units.hpp"
+#include "L1_Peripheral/pwm.hpp"
+#include "L1_Peripheral/gpio.hpp"
+#include "L3_Application/Robotics/Common/MagneticEncoder.hpp"
+
+namespace sjsu
+{
+namespace robotics
+{
+static constexpr float kPwmHigh = 1.0f;
+static constexpr float kPwmLow  = 1.0f;
+// need a pwm phase, and gpio enable
+class Drv8801
+{
+ public:
+  explicit constexpr Drv8801(sjsu::Pwm & enable,
+                             sjsu::Gpio & direction_pin,
+                             sjsu::Gpio & mode_pin)
+      : enable_(enable), direction_pin_(direction_pin), mode_pin_(mode_pin)
+  {
+  }
+  void Initialize()
+  {
+    enable_.Initialize(50_Hz);
+    direction_pin_.SetAsOutput();
+    mode_pin_.SetAsOutput();
+    mode_pin_.SetLow();
+  }
+  void TurnForward()
+  {
+    LOG_INFO("Direction pin high, PWM high");
+    direction_pin_.SetHigh();
+    enable_.SetDutyCycle(kPwmHigh);
+  }
+  void TurnBackward()
+  {
+    LOG_INFO("Direction pin low, PWM high");
+    direction_pin_.SetLow();
+    enable_.SetDutyCycle(kPwmHigh);
+  }
+  void Stop()
+  {
+    LOG_INFO("PWM low");
+    enable_.SetDutyCycle(kPwmLow);
+  }
+
+ private:
+  // Pwm pin to enable the motor to move. When it is 0 the motor stops.
+  const sjsu::Pwm & enable_;
+  // Gpio pin to control the movement. If it is high the motor goes forward, if
+  // it is low the motor goes backward
+  const sjsu::Gpio & direction_pin_;
+  const sjsu::Gpio & mode_pin_;
+};
+}  // namespace robotics
+}  // namespace sjsu

library/L3_Application/Robotics/Common/MagneticEncoder.hpp

@@ -0,0 +1,60 @@
+#pragma once
+
+#include <cstdint>
+#include "utility/log.hpp"
+#include "L1_Peripheral/i2c.hpp"
+
+// Change these address values later
+
+namespace sjsu
+{
+namespace robotics
+{
+class MagneticEncoder
+{
+ public:
+  static constexpr uint8_t kMagneticEncoderAddress   = 0x36;
+  static constexpr uint8_t kRawAngleHighRegister     = 0x0E;
+  static constexpr uint8_t kRawAngleLowRegister      = 0x0F;
+  static constexpr uint8_t kMsbShift                 = 8;
+  static constexpr units::angle::degree_t kZeroAngle = 0_deg;
+  explicit constexpr MagneticEncoder(sjsu::I2c & i2c)
+      : current_angle_(kZeroAngle), i2c_(i2c)
+  {
+  }
+  void Initialize()
+  {
+    i2c_.Initialize();
+    // idk what goes here figure the address out later
+    i2c_.Write(kMagneticEncoderAddress, { 0x1 });
+  }
+
+  units::angle::degree_t GetAngle() const
+  {
+    // i have no idea how to read with i2c, gotta read datasheet here
+    int tilt_reading;
+    int16_t axis_tilt;
+    LOG_INFO("before: %d", axis_tilt);
+    uint8_t tilt_val[2];
+    i2c_.WriteThenRead(kMagneticEncoderAddress,
+                       { kRawAngleLowRegister },
+                       tilt_val,
+                       sizeof(tilt_val));
+    tilt_reading = (tilt_val[0] << kMsbShift) | tilt_val[1];
+    axis_tilt    = static_cast<int16_t>(tilt_reading);
+    LOG_INFO("after: %d %d", axis_tilt, tilt_val[1]);
+    units::angle::degree_t angle(axis_tilt);
+    return angle;
+  }
+  void SetZero()
+  {
+    // do some i2c here to set the current angle to zero.
+    current_angle_ = kZeroAngle;
+  }
+
+ private:
+  units::angle::degree_t current_angle_;
+  const sjsu::I2c & i2c_;
+};
+}  // namespace robotics
+}  // namespace sjsu

library/L3_Application/Robotics/Common/Params.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+namespace sjsu
+{
+namespace robotics
+{
+// represents type of things sent from mission control. will build this out.
+struct ParamsStruct
+{
+  // drive mode e.g. crab, debug, spin, drive
+  int drive_mode;
+  // angle of a given wheel
+  double wheel_angle;
+  // angle of a given arm component
+  double arm_angle;
+  // speed of hub motor
+  double wheel_speed;
+  // true if wheel should run in reverse or not
+  bool drive_reverse;
+  // true/false depending if we are controlling wheel a
+  bool wheel_a;
+  // true/false depending if we are controlling wheel b
+  bool wheel_b;
+  // true/false depending if we are controlling wheel c
+  bool wheel_c;
+  // true/false depending if we are controlling wrist
+  bool wrist;
+  // true/false depending if we are controlling shoulder
+  bool shoulder;
+  // true/false depending if we are controlling rotunda
+  bool rotunda;
+};
+}  // namespace robotics
+}  // namespace sjsu