Andrew Hellrigel / SwerveModule

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.

Dependents:   SwerveDriveRobot

SwerveModule.h@2:1ae2ebcba406, 2022-04-29 (annotated)

Committer:
andrewh33
Date:
Fri Apr 29 11:11:38 2022 +0000
Revision:
2:1ae2ebcba406
Parent:
1:05a5529f9c0d 
update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
andrewh33 0:197b43f2f054 1 #ifndef SWERVEMODULE_H
andrewh33 0:197b43f2f054 2 #define SWERVEMODULE_H
andrewh33 0:197b43f2f054 3
andrewh33 0:197b43f2f054 4 #include "mbed.h"
andrewh33 0:197b43f2f054 5 #include "SteerMotor.h"
andrewh33 0:197b43f2f054 6 #include "DriveMotor.h"
andrewh33 0:197b43f2f054 7
andrewh33 0:197b43f2f054 8 class SwerveModule {
andrewh33 0:197b43f2f054 9 private:
andrewh33 0:197b43f2f054 10 SteerMotor* _steer;
andrewh33 0:197b43f2f054 11 DriveMotor* _drive;
andrewh33 0:197b43f2f054 12
andrewh33 0:197b43f2f054 13 public:
andrewh33 0:197b43f2f054 14 SwerveModule(SteerMotor* steer, DriveMotor* drive) : _steer(steer), _drive(drive) {}
andrewh33 0:197b43f2f054 15
andrewh33 0:197b43f2f054 16 void begin() {
andrewh33 0:197b43f2f054 17 _steer->zero();
andrewh33 0:197b43f2f054 18 _drive->kill();
andrewh33 0:197b43f2f054 19 }
andrewh33 0:197b43f2f054 20
andrewh33 0:197b43f2f054 21 // speed is number from 0-1, angle is in degrees from 0-360
andrewh33 0:197b43f2f054 22 void drive(float speed, float angle){
andrewh33 0:197b43f2f054 23 float curr_angle = _steer->get_angle();
andrewh33 0:197b43f2f054 24 if ((angle > curr_angle + 90.0f && angle < curr_angle + 270.0f) ||
andrewh33 0:197b43f2f054 25 (angle > curr_angle - 270.0f && angle < curr_angle - 90.0f)){
andrewh33 0:197b43f2f054 26 angle += (angle < 180.0f) ? 180.0f : -180.0f;
andrewh33 0:197b43f2f054 27 _drive->set_dir(0);
andrewh33 0:197b43f2f054 28 } else {
andrewh33 0:197b43f2f054 29 _drive->set_dir(1);
andrewh33 0:197b43f2f054 30 }
andrewh33 1:05a5529f9c0d 31 if (speed < 0.01){
andrewh33 1:05a5529f9c0d 32 _steer->set_angle(curr_angle);
andrewh33 1:05a5529f9c0d 33 } else {
andrewh33 1:05a5529f9c0d 34 _steer->set_angle(angle);
andrewh33 1:05a5529f9c0d 35 }
andrewh33 0:197b43f2f054 36 _drive->set_speed(speed);
andrewh33 0:197b43f2f054 37 }
andrewh33 0:197b43f2f054 38
andrewh33 0:197b43f2f054 39 void kill(){
andrewh33 0:197b43f2f054 40 _steer->kill();
andrewh33 0:197b43f2f054 41 _drive->kill();
andrewh33 0:197b43f2f054 42 }
andrewh33 0:197b43f2f054 43
andrewh33 0:197b43f2f054 44 void update(uint32_t curr_time){
andrewh33 0:197b43f2f054 45 _steer->update(curr_time);
andrewh33 0:197b43f2f054 46 }
andrewh33 0:197b43f2f054 47
andrewh33 2:1ae2ebcba406 48 bool is_on_target(){
andrewh33 2:1ae2ebcba406 49 return _steer->is_on_target();
andrewh33 2:1ae2ebcba406 50 }
andrewh33 2:1ae2ebcba406 51
andrewh33 0:197b43f2f054 52 };
andrewh33 0:197b43f2f054 53
andrewh33 0:197b43f2f054 54 #endif