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@3:4f35942aee8e, 2022-04-28 (annotated)

Committer:
rchen390
Date:
Thu Apr 28 20:25:37 2022 +0000
Revision:
3:4f35942aee8e
Parent:
2:69d290bd4b7d
Child:
4:7be1da65326d
Added working receiver code over python to mbed for xbox controller control

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 0:c6a3095b5843 8 Timer t;
andrewh33 0:c6a3095b5843 9
rchen390 2:69d290bd4b7d 10
rchen390 2:69d290bd4b7d 11 // UART
rchen390 2:69d290bd4b7d 12 Serial pc(USBTX, USBRX);
rchen390 2:69d290bd4b7d 13 RawSerial bluetooth(p13,p14);
rchen390 2:69d290bd4b7d 14
rchen390 2:69d290bd4b7d 15
andrewh33 0:c6a3095b5843 16 // Steer Motor Pins
andrewh33 0:c6a3095b5843 17 DigitalOut bl_sdir(p9);
andrewh33 0:c6a3095b5843 18 DigitalOut bl_step(p10);
andrewh33 0:c6a3095b5843 19 DigitalOut br_sdir(p11);
andrewh33 0:c6a3095b5843 20 DigitalOut br_step(p12);
andrewh33 0:c6a3095b5843 21 DigitalOut fl_sdir(p7);
andrewh33 0:c6a3095b5843 22 DigitalOut fl_step(p8);
andrewh33 0:c6a3095b5843 23 DigitalOut fr_sdir(p5);
andrewh33 0:c6a3095b5843 24 DigitalOut fr_step(p6);
andrewh33 0:c6a3095b5843 25
andrewh33 0:c6a3095b5843 26 // Drive Motor Pins
andrewh33 0:c6a3095b5843 27 DigitalOut bl_ddir(p20);
andrewh33 0:c6a3095b5843 28 PwmOut bl_pwm(p21);
andrewh33 0:c6a3095b5843 29 DigitalOut br_ddir(p19);
andrewh33 0:c6a3095b5843 30 PwmOut br_pwm(p22);
andrewh33 0:c6a3095b5843 31 DigitalOut fl_ddir(p18);
andrewh33 0:c6a3095b5843 32 PwmOut fl_pwm(p23);
andrewh33 0:c6a3095b5843 33 DigitalOut fr_ddir(p17);
andrewh33 0:c6a3095b5843 34 PwmOut fr_pwm(p24);
andrewh33 0:c6a3095b5843 35
andrewh33 0:c6a3095b5843 36 // Motors
andrewh33 0:c6a3095b5843 37 SteerMotor bl_steer(bl_sdir, bl_step);
andrewh33 0:c6a3095b5843 38 SteerMotor br_steer(br_sdir, br_step);
andrewh33 0:c6a3095b5843 39 SteerMotor fl_steer(fl_sdir, fl_step);
andrewh33 0:c6a3095b5843 40 SteerMotor fr_steer(fr_sdir, fr_step);
andrewh33 0:c6a3095b5843 41 DriveMotor bl_drive(bl_ddir, bl_pwm);
andrewh33 0:c6a3095b5843 42 DriveMotor br_drive(br_ddir, br_pwm);
andrewh33 0:c6a3095b5843 43 DriveMotor fl_drive(fl_ddir, fl_pwm);
andrewh33 0:c6a3095b5843 44 DriveMotor fr_drive(fr_ddir, fr_pwm);
andrewh33 0:c6a3095b5843 45
andrewh33 0:c6a3095b5843 46 // Modules
andrewh33 0:c6a3095b5843 47 SwerveModule bl_module(&bl_steer, &bl_drive);
andrewh33 0:c6a3095b5843 48 SwerveModule br_module(&br_steer, &br_drive);
andrewh33 0:c6a3095b5843 49 SwerveModule fl_module(&fl_steer, &fl_drive);
andrewh33 0:c6a3095b5843 50 SwerveModule fr_module(&fr_steer, &fr_drive);
andrewh33 0:c6a3095b5843 51
andrewh33 0:c6a3095b5843 52 SwerveDrive swerve(&fl_module, &fr_module, &bl_module, &br_module);
andrewh33 0:c6a3095b5843 53
rchen390 2:69d290bd4b7d 54 PwmOut myled1(LED1);
rchen390 2:69d290bd4b7d 55 PwmOut myled2(LED2);
rchen390 2:69d290bd4b7d 56 PwmOut myled3(LED3);
rchen390 2:69d290bd4b7d 57 PwmOut myled4(LED4);
andrewh33 0:c6a3095b5843 58
andrewh33 0:c6a3095b5843 59 uint32_t curr_time = 0; // In microseconds
andrewh33 0:c6a3095b5843 60
rchen390 2:69d290bd4b7d 61 //l_joy_x, l_joy_y, r_joy_x
rchen390 2:69d290bd4b7d 62 float controller[CTRL_CNT] = {0};
rchen390 2:69d290bd4b7d 63
rchen390 2:69d290bd4b7d 64
andrewh33 0:c6a3095b5843 65 int main() {
rchen390 3:4f35942aee8e 66 t.start();
rchen390 3:4f35942aee8e 67 swerve.begin();
rchen390 2:69d290bd4b7d 68 myled1 = 0;
rchen390 2:69d290bd4b7d 69 myled2 = 0;
rchen390 2:69d290bd4b7d 70 myled3 = 0;
rchen390 2:69d290bd4b7d 71 myled4 = 0;
rchen390 2:69d290bd4b7d 72 pc.baud(9600);
rchen390 2:69d290bd4b7d 73 bluetooth.baud(9600);
rchen390 2:69d290bd4b7d 74
rchen390 2:69d290bd4b7d 75
andrewh33 0:c6a3095b5843 76
rchen390 2:69d290bd4b7d 77 if (pc.writeable()) {
rchen390 2:69d290bd4b7d 78 pc.putc('s');
rchen390 2:69d290bd4b7d 79 }
rchen390 2:69d290bd4b7d 80
rchen390 3:4f35942aee8e 81 uint32_t prev_time = 0;
rchen390 3:4f35942aee8e 82 uint32_t interval = .1*1000000;
rchen390 2:69d290bd4b7d 83
rchen390 2:69d290bd4b7d 84 int counter = 0;
andrewh33 0:c6a3095b5843 85 while(1) {
rchen390 3:4f35942aee8e 86 curr_time = t.read_us();
rchen390 2:69d290bd4b7d 87 // swerve.update(curr_time);
rchen390 2:69d290bd4b7d 88 // swerve.drive(0.1f, 0.1f, 0.1f);
rchen390 3:4f35942aee8e 89 if ((curr_time - prev_time) > interval) {
rchen390 3:4f35942aee8e 90 myled4 = !myled4;
rchen390 3:4f35942aee8e 91 prev_time = curr_time;
rchen390 3:4f35942aee8e 92 }
rchen390 3:4f35942aee8e 93 if (pc.readable()) {
rchen390 3:4f35942aee8e 94 while (pc.getc() != 'a') {
rchen390 2:69d290bd4b7d 95 }
rchen390 3:4f35942aee8e 96 for (int ind = 0; ind < CTRL_CNT; ind++) {
rchen390 3:4f35942aee8e 97 float f;
rchen390 3:4f35942aee8e 98 char b[4];
rchen390 3:4f35942aee8e 99 for (int byte_index = 0; byte_index < 4; byte_index++) {
rchen390 3:4f35942aee8e 100 b[byte_index] = pc.getc();
rchen390 2:69d290bd4b7d 101 }
rchen390 3:4f35942aee8e 102 memcpy(&f, &b, sizeof(f));
rchen390 3:4f35942aee8e 103 controller[ind] = f;
rchen390 2:69d290bd4b7d 104 }
rchen390 2:69d290bd4b7d 105 }
rchen390 2:69d290bd4b7d 106
rchen390 2:69d290bd4b7d 107
rchen390 2:69d290bd4b7d 108 myled1 = abs(controller[0]);
rchen390 2:69d290bd4b7d 109 myled2 = abs(controller[1]);
rchen390 2:69d290bd4b7d 110 myled3 = abs(controller[2]);
andrewh33 0:c6a3095b5843 111 }
rchen390 2:69d290bd4b7d 112
andrewh33 0:c6a3095b5843 113 }
rchen390 2:69d290bd4b7d 114
rchen390 2:69d290bd4b7d 115