Andrew Hellrigel / Mbed 2 deprecated SwerveDriveRobot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   DriveMotor mbed SteerMotor SwerveDrive SwerveModule

main.cpp@5:164c27f5399b, 2022-04-29 (annotated)

Committer:
andrewh33
Date:
Fri Apr 29 11:12:00 2022 +0000
Revision:
5:164c27f5399b
Parent:
4:7be1da65326d 
Update: Doesn't work fully

Who changed what in which revision?

UserRevisionLine numberNew contents of line
andrewh33 0:c6a3095b5843 1 #include "mbed.h"
andrewh33 0:c6a3095b5843 2 #include "SteerMotor.h"
andrewh33 0:c6a3095b5843 3 #include "DriveMotor.h"
andrewh33 0:c6a3095b5843 4 #include "SwerveModule.h"
andrewh33 0:c6a3095b5843 5 #include "SwerveDrive.h"
andrewh33 0:c6a3095b5843 6
rchen390 2:69d290bd4b7d 7 #define CTRL_CNT 3 // number of 4 byte packages sent over bluetooth
andrewh33 5:164c27f5399b 8 #define MAX_SPEED 0.5
andrewh33 4:7be1da65326d 9
andrewh33 0:c6a3095b5843 10 Timer t;
andrewh33 0:c6a3095b5843 11
rchen390 2:69d290bd4b7d 12
rchen390 2:69d290bd4b7d 13 // UART
rchen390 2:69d290bd4b7d 14 Serial pc(USBTX, USBRX);
andrewh33 4:7be1da65326d 15 RawSerial esp8266(p13,p14);
rchen390 2:69d290bd4b7d 16
rchen390 2:69d290bd4b7d 17
andrewh33 0:c6a3095b5843 18 // Steer Motor Pins
andrewh33 4:7be1da65326d 19 DigitalOut bl_sdir(p10);
andrewh33 4:7be1da65326d 20 DigitalOut bl_step(p9);
andrewh33 4:7be1da65326d 21 DigitalOut br_sdir(p12);
andrewh33 4:7be1da65326d 22 DigitalOut br_step(p11);
andrewh33 4:7be1da65326d 23 DigitalOut fl_sdir(p8);
andrewh33 4:7be1da65326d 24 DigitalOut fl_step(p7);
andrewh33 4:7be1da65326d 25 DigitalOut fr_sdir(p6);
andrewh33 4:7be1da65326d 26 DigitalOut fr_step(p5);
andrewh33 0:c6a3095b5843 27
andrewh33 0:c6a3095b5843 28 // Drive Motor Pins
andrewh33 4:7be1da65326d 29 DigitalOut br_ddir(p20);
andrewh33 4:7be1da65326d 30 PwmOut br_pwm(p21);
andrewh33 4:7be1da65326d 31 DigitalOut bl_ddir(p19);
andrewh33 4:7be1da65326d 32 PwmOut bl_pwm(p22);
andrewh33 0:c6a3095b5843 33 DigitalOut fl_ddir(p18);
andrewh33 0:c6a3095b5843 34 PwmOut fl_pwm(p23);
andrewh33 0:c6a3095b5843 35 DigitalOut fr_ddir(p17);
andrewh33 0:c6a3095b5843 36 PwmOut fr_pwm(p24);
andrewh33 0:c6a3095b5843 37
andrewh33 0:c6a3095b5843 38 // Motors
andrewh33 0:c6a3095b5843 39 SteerMotor bl_steer(bl_sdir, bl_step);
andrewh33 0:c6a3095b5843 40 SteerMotor br_steer(br_sdir, br_step);
andrewh33 0:c6a3095b5843 41 SteerMotor fl_steer(fl_sdir, fl_step);
andrewh33 0:c6a3095b5843 42 SteerMotor fr_steer(fr_sdir, fr_step);
andrewh33 0:c6a3095b5843 43 DriveMotor bl_drive(bl_ddir, bl_pwm);
andrewh33 0:c6a3095b5843 44 DriveMotor br_drive(br_ddir, br_pwm);
andrewh33 0:c6a3095b5843 45 DriveMotor fl_drive(fl_ddir, fl_pwm);
andrewh33 0:c6a3095b5843 46 DriveMotor fr_drive(fr_ddir, fr_pwm);
andrewh33 0:c6a3095b5843 47
andrewh33 0:c6a3095b5843 48 // Modules
andrewh33 0:c6a3095b5843 49 SwerveModule bl_module(&bl_steer, &bl_drive);
andrewh33 0:c6a3095b5843 50 SwerveModule br_module(&br_steer, &br_drive);
andrewh33 0:c6a3095b5843 51 SwerveModule fl_module(&fl_steer, &fl_drive);
andrewh33 0:c6a3095b5843 52 SwerveModule fr_module(&fr_steer, &fr_drive);
andrewh33 0:c6a3095b5843 53
andrewh33 5:164c27f5399b 54 //DigitalOut en(p15);
andrewh33 5:164c27f5399b 55
andrewh33 0:c6a3095b5843 56 SwerveDrive swerve(&fl_module, &fr_module, &bl_module, &br_module);
andrewh33 0:c6a3095b5843 57
rchen390 2:69d290bd4b7d 58 PwmOut myled1(LED1);
rchen390 2:69d290bd4b7d 59 PwmOut myled2(LED2);
rchen390 2:69d290bd4b7d 60 PwmOut myled3(LED3);
rchen390 2:69d290bd4b7d 61 PwmOut myled4(LED4);
andrewh33 0:c6a3095b5843 62
andrewh33 0:c6a3095b5843 63 uint32_t curr_time = 0; // In microseconds
andrewh33 4:7be1da65326d 64 uint32_t prev_time = 0; // Also in microseconds
andrewh33 4:7be1da65326d 65 uint8_t ind = 0;
andrewh33 4:7be1da65326d 66 uint8_t byte_index = 0;
andrewh33 4:7be1da65326d 67 float f;
andrewh33 4:7be1da65326d 68 char b[4];
andrewh33 0:c6a3095b5843 69
rchen390 2:69d290bd4b7d 70 //l_joy_x, l_joy_y, r_joy_x
rchen390 2:69d290bd4b7d 71 float controller[CTRL_CNT] = {0};
rchen390 2:69d290bd4b7d 72
rchen390 2:69d290bd4b7d 73
andrewh33 4:7be1da65326d 74 int main() {
rchen390 3:4f35942aee8e 75 t.start();
rchen390 3:4f35942aee8e 76 swerve.begin();
andrewh33 4:7be1da65326d 77 swerve.kill();
andrewh33 4:7be1da65326d 78
andrewh33 4:7be1da65326d 79 fr_drive.set_multiplier(1.00 * MAX_SPEED);
andrewh33 4:7be1da65326d 80 fl_drive.set_multiplier(0.68 * MAX_SPEED);
andrewh33 4:7be1da65326d 81 bl_drive.set_multiplier(0.56 * MAX_SPEED);
andrewh33 4:7be1da65326d 82 br_drive.set_multiplier(0.77 * MAX_SPEED);
andrewh33 4:7be1da65326d 83
rchen390 2:69d290bd4b7d 84 myled1 = 0;
rchen390 2:69d290bd4b7d 85 myled2 = 0;
rchen390 2:69d290bd4b7d 86 myled3 = 0;
rchen390 2:69d290bd4b7d 87 myled4 = 0;
rchen390 2:69d290bd4b7d 88 pc.baud(9600);
andrewh33 4:7be1da65326d 89 esp8266.baud(9600);
rchen390 2:69d290bd4b7d 90
andrewh33 0:c6a3095b5843 91 while(1) {
rchen390 3:4f35942aee8e 92 curr_time = t.read_us();
andrewh33 4:7be1da65326d 93
andrewh33 4:7be1da65326d 94 swerve.update(curr_time);
andrewh33 5:164c27f5399b 95 swerve.drive(controller[1], controller[0], controller[2], curr_time);
andrewh33 4:7be1da65326d 96
andrewh33 4:7be1da65326d 97 if (esp8266.readable()) {
andrewh33 4:7be1da65326d 98 char temp = esp8266.getc();
andrewh33 4:7be1da65326d 99 if (byte_index > 3) {
rchen390 3:4f35942aee8e 100 memcpy(&f, &b, sizeof(f));
rchen390 3:4f35942aee8e 101 controller[ind] = f;
andrewh33 4:7be1da65326d 102 byte_index = 0;
andrewh33 4:7be1da65326d 103 ind = ind == 2 ? 0 : ind + 1;
andrewh33 4:7be1da65326d 104 }
andrewh33 5:164c27f5399b 105 if (temp == 'q'){
andrewh33 4:7be1da65326d 106 prev_time = t.read_us();
andrewh33 4:7be1da65326d 107 byte_index = 0;
andrewh33 4:7be1da65326d 108 ind = 0;
andrewh33 5:164c27f5399b 109 } else if (temp == 'a'){
andrewh33 5:164c27f5399b 110 swerve.begin(); // When press A, zero wheels
andrewh33 4:7be1da65326d 111 } else {
andrewh33 4:7be1da65326d 112 b[byte_index] = temp;
andrewh33 4:7be1da65326d 113 byte_index++;
andrewh33 4:7be1da65326d 114 }
andrewh33 4:7be1da65326d 115 } else {
andrewh33 4:7be1da65326d 116 if (curr_time - prev_time > 100000){
andrewh33 4:7be1da65326d 117 myled4 = 0;
andrewh33 4:7be1da65326d 118 for (int ind = 0; ind < CTRL_CNT; ind++) {
andrewh33 4:7be1da65326d 119 controller[ind] = 0;
andrewh33 4:7be1da65326d 120 }
andrewh33 4:7be1da65326d 121 swerve.kill();
rchen390 2:69d290bd4b7d 122 }
rchen390 2:69d290bd4b7d 123 }
rchen390 2:69d290bd4b7d 124
rchen390 2:69d290bd4b7d 125 myled1 = abs(controller[0]);
rchen390 2:69d290bd4b7d 126 myled2 = abs(controller[1]);
rchen390 2:69d290bd4b7d 127 myled3 = abs(controller[2]);
andrewh33 0:c6a3095b5843 128 }
andrewh33 0:c6a3095b5843 129 }
rchen390 2:69d290bd4b7d 130
rchen390 2:69d290bd4b7d 131