Diff: main.cpp

Revision:

0:c6a3095b5843

Child:

2:69d290bd4b7d

diff -r 000000000000 -r c6a3095b5843 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Apr 23 23:55:36 2022 +0000
@@ -0,0 +1,62 @@
+#include "mbed.h"
+#include "SteerMotor.h"
+#include "DriveMotor.h"
+#include "SwerveModule.h"
+#include "SwerveDrive.h"
+
+Timer t;
+
+// Steer Motor Pins
+DigitalOut bl_sdir(p9);
+DigitalOut bl_step(p10);
+DigitalOut br_sdir(p11);
+DigitalOut br_step(p12);
+DigitalOut fl_sdir(p7);
+DigitalOut fl_step(p8);
+DigitalOut fr_sdir(p5);
+DigitalOut fr_step(p6);
+
+// Drive Motor Pins
+DigitalOut bl_ddir(p20);
+PwmOut bl_pwm(p21);
+DigitalOut br_ddir(p19);
+PwmOut br_pwm(p22);
+DigitalOut fl_ddir(p18);
+PwmOut fl_pwm(p23);
+DigitalOut fr_ddir(p17);
+PwmOut fr_pwm(p24);
+
+// Motors
+SteerMotor bl_steer(bl_sdir, bl_step);
+SteerMotor br_steer(br_sdir, br_step);
+SteerMotor fl_steer(fl_sdir, fl_step);
+SteerMotor fr_steer(fr_sdir, fr_step);
+DriveMotor bl_drive(bl_ddir, bl_pwm);
+DriveMotor br_drive(br_ddir, br_pwm);
+DriveMotor fl_drive(fl_ddir, fl_pwm);
+DriveMotor fr_drive(fr_ddir, fr_pwm);
+
+// Modules
+SwerveModule bl_module(&bl_steer, &bl_drive);
+SwerveModule br_module(&br_steer, &br_drive);
+SwerveModule fl_module(&fl_steer, &fl_drive);
+SwerveModule fr_module(&fr_steer, &fr_drive);
+
+SwerveDrive swerve(&fl_module, &fr_module, &bl_module, &br_module);
+
+DigitalOut myled(LED1);
+
+uint32_t curr_time = 0; // In microseconds
+
+int main() {
+    t.start();
+    swerve.begin();
+
+    
+    while(1) {
+        curr_time = t.read_us();
+        swerve.update(curr_time);
+        swerve.drive(0.1f, 0.1f, 0.1f);
+        myled = 1;
+    }
+}