Diff: SwerveModule.h

Revision:

0:197b43f2f054

Child:

1:05a5529f9c0d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SwerveModule.h	Sat Apr 23 23:55:25 2022 +0000
@@ -0,0 +1,46 @@
+#ifndef SWERVEMODULE_H
+#define SWERVEMODULE_H
+
+#include "mbed.h"
+#include "SteerMotor.h"
+#include "DriveMotor.h"
+
+class SwerveModule {
+private:
+    SteerMotor* _steer;
+    DriveMotor* _drive;
+
+public:
+    SwerveModule(SteerMotor* steer, DriveMotor* drive) : _steer(steer), _drive(drive) {}
+    
+    void begin() {
+        _steer->zero();
+        _drive->kill();
+    }
+    
+    // speed is number from 0-1, angle is in degrees from 0-360
+    void drive(float speed, float angle){
+        float curr_angle = _steer->get_angle();
+        if ((angle > curr_angle + 90.0f && angle < curr_angle + 270.0f) ||
+            (angle > curr_angle - 270.0f && angle < curr_angle - 90.0f)){
+            angle += (angle < 180.0f) ? 180.0f : -180.0f;
+            _drive->set_dir(0);
+        } else {
+            _drive->set_dir(1);
+        }
+        _steer->set_angle(angle);
+        _drive->set_speed(speed);
+    }
+    
+    void kill(){
+        _steer->kill();
+        _drive->kill();
+    }
+    
+    void update(uint32_t curr_time){
+        _steer->update(curr_time);
+    }
+    
+};
+
+#endif
\ No newline at end of file