Pat McC / thruster

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.

thruster.cpp@4:d32183587460, 2021-04-09 (annotated)

Committer:
pmmccorkell
Date:
Fri Apr 09 11:38:56 2021 +0000
Revision:
4:d32183587460
Parent:
3:02a9d402226d
Child:
5:e0c525588568
asdf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmmccorkell 0:50cdd1590925 1 /*
pmmccorkell 0:50cdd1590925 2 * Thrust class for LPC1768
pmmccorkell 0:50cdd1590925 3 * US Naval Academy
pmmccorkell 0:50cdd1590925 4 * Robotics and Control TSD
pmmccorkell 0:50cdd1590925 5 * Patrick McCorkell
pmmccorkell 0:50cdd1590925 6 *
pmmccorkell 0:50cdd1590925 7 * Created: 2020 Nov 23
pmmccorkell 0:50cdd1590925 8 *
pmmccorkell 0:50cdd1590925 9 */
pmmccorkell 0:50cdd1590925 10
pmmccorkell 4:d32183587460 11 #include "mbed.h"
pmmccorkell 0:50cdd1590925 12 #include "thruster.h"
pmmccorkell 0:50cdd1590925 13
pmmccorkell 0:50cdd1590925 14 //Instantiation accepts PWM pin and direction of prop blade
pmmccorkell 0:50cdd1590925 15 //Direction is -1 or 1.
pmmccorkell 0:50cdd1590925 16 //1 for normal, -1 if blade reversed.
pmmccorkell 0:50cdd1590925 17 Thruster::Thruster(PinName pin, float dir) : _pwm(pin), _d(dir) {
pmmccorkell 0:50cdd1590925 18 _lock=0; // emergency lockout, default is 0
pmmccorkell 0:50cdd1590925 19 _pin=pin; // PWM pin
pmmccorkell 0:50cdd1590925 20 _base_pw=1.5; // 1.5ms
pmmccorkell 0:50cdd1590925 21 set_pw(_base_pw); // set PWM to 1.5ms
pmmccorkell 0:50cdd1590925 22 _period=2.5; // 2.5ms
pmmccorkell 0:50cdd1590925 23 set_period(_period); // set period to 2.5ms (400Hz)
pmmccorkell 4:d32183587460 24 _max=150; // max PWM value in us
pmmccorkell 1:94191f7e9b27 25 //printf("Thruster: %f\r\n",d);
pmmccorkell 0:50cdd1590925 26 }
pmmccorkell 0:50cdd1590925 27
pmmccorkell 0:50cdd1590925 28 //Sets Event for Emergency Stop and sets lockout to set_speed() function.
pmmccorkell 0:50cdd1590925 29 void Thruster::setEvent() {
pmmccorkell 0:50cdd1590925 30 _lock=1; // set _lock flag to lockout set_speed functionality.
pmmccorkell 0:50cdd1590925 31 set_pw(_base_pw); // write the neutral PWM value.
pmmccorkell 0:50cdd1590925 32 }
pmmccorkell 0:50cdd1590925 33
pmmccorkell 0:50cdd1590925 34 //Clears Event for Emergency Stop of thruster and removes lockout from set_speed() function.
pmmccorkell 0:50cdd1590925 35 void Thruster::clearEvent() {
pmmccorkell 0:50cdd1590925 36 _lock=0; // set _lock flag back to 0, enabling set_speed functionality.
pmmccorkell 0:50cdd1590925 37 }
pmmccorkell 0:50cdd1590925 38
pmmccorkell 0:50cdd1590925 39 //Set PWM period in ms.
pmmccorkell 0:50cdd1590925 40 void Thruster::set_period(double thruster_time) {
pmmccorkell 0:50cdd1590925 41 _period=thruster_time;
pmmccorkell 0:50cdd1590925 42 _pwm.period(_period/1000);
pmmccorkell 0:50cdd1590925 43 }
pmmccorkell 0:50cdd1590925 44
pmmccorkell 0:50cdd1590925 45 //Set PWM pulsewidth in ms
pmmccorkell 0:50cdd1590925 46 void Thruster::set_pw(double thruster_pw) {
pmmccorkell 0:50cdd1590925 47 double s_pw=(thruster_pw/1000);
pmmccorkell 0:50cdd1590925 48 printf("log: set_pw: %f\r\n",s_pw);
pmmccorkell 0:50cdd1590925 49 _pwm.pulsewidth(s_pw);
pmmccorkell 0:50cdd1590925 50 }
pmmccorkell 0:50cdd1590925 51
pmmccorkell 0:50cdd1590925 52 //Returns PWM pulsewidth in ms.
pmmccorkell 0:50cdd1590925 53 double Thruster::get_pw() {
pmmccorkell 0:50cdd1590925 54 //read duty cycle times period
pmmccorkell 0:50cdd1590925 55 double g_pw = (_pwm.read()*_period);
pmmccorkell 0:50cdd1590925 56 //printf(" get_pw: %f, ",g_pw);
pmmccorkell 0:50cdd1590925 57 return g_pw;
pmmccorkell 0:50cdd1590925 58 }
pmmccorkell 0:50cdd1590925 59
pmmccorkell 0:50cdd1590925 60 //Returns PWM output relative to 1.5ms.
pmmccorkell 0:50cdd1590925 61 double Thruster::get_speed() {
pmmccorkell 0:50cdd1590925 62 double g_speed = (get_pw()-_base_pw);
pmmccorkell 0:50cdd1590925 63 //printf("get_speed: %f, ",g_speed);
pmmccorkell 0:50cdd1590925 64 return g_speed;
pmmccorkell 0:50cdd1590925 65 }
pmmccorkell 0:50cdd1590925 66
pmmccorkell 0:50cdd1590925 67 //formats PWM as an 2 uint16_t joined to make uint32_t for serial data streaming
pmmccorkell 0:50cdd1590925 68 //MSB uint16_t indicates direction, 0 for positive, 1 for negative.
pmmccorkell 4:d32183587460 69 //LSB uint16_t is 10us resolution of PWM
pmmccorkell 0:50cdd1590925 70 uint32_t Thruster::thruster_data() {
pmmccorkell 0:50cdd1590925 71 double speed=get_speed();
pmmccorkell 0:50cdd1590925 72 uint32_t dir=0x0;
pmmccorkell 0:50cdd1590925 73 uint32_t data=0x0;
pmmccorkell 0:50cdd1590925 74 if (speed<0) dir =0x00010000;
pmmccorkell 0:50cdd1590925 75 data=static_cast<unsigned int>(abs(int(speed*100000)));
pmmccorkell 0:50cdd1590925 76 data=data+dir;
pmmccorkell 0:50cdd1590925 77 return data;
pmmccorkell 0:50cdd1590925 78 }
pmmccorkell 0:50cdd1590925 79
pmmccorkell 4:d32183587460 80 //Accepts adjustment to max value of pw [x,500] us for set_speed() function.
pmmccorkell 0:50cdd1590925 81 //Returns 1 if successful.
pmmccorkell 4:d32183587460 82 void Thruster::set_max(int new_max) {
pmmccorkell 4:d32183587460 83 if (new_max<=500) {
pmmccorkell 0:50cdd1590925 84 _max=new_max;
pmmccorkell 0:50cdd1590925 85 }
pmmccorkell 0:50cdd1590925 86 }
pmmccorkell 0:50cdd1590925 87
pmmccorkell 4:d32183587460 88 int Thruster::get_max() {
pmmccorkell 4:d32183587460 89 int buffer = _max;
pmmccorkell 4:d32183587460 90 return buffer;
pmmccorkell 4:d32183587460 91 }
pmmccorkell 4:d32183587460 92
pmmccorkell 4:d32183587460 93 //Accepts adjustment to pw [-500,500] us that is added to 1.5ms.
pmmccorkell 0:50cdd1590925 94 //Returns 1 if successful.
pmmccorkell 0:50cdd1590925 95 int Thruster::set_speed(double speed_pw) {
pmmccorkell 0:50cdd1590925 96 int returnval = 0;
pmmccorkell 0:50cdd1590925 97 if (_lock==1) {
pmmccorkell 0:50cdd1590925 98 set_pw(_base_pw);
pmmccorkell 0:50cdd1590925 99 }
pmmccorkell 0:50cdd1590925 100 else if (abs(speed_pw) > _max) {
pmmccorkell 3:02a9d402226d 101 //print("max speed exceeded");
pmmccorkell 1:94191f7e9b27 102 returnval=0;
pmmccorkell 0:50cdd1590925 103 }
pmmccorkell 0:50cdd1590925 104 else {
pmmccorkell 0:50cdd1590925 105 double tolerance_pw=0.001;
pmmccorkell 0:50cdd1590925 106 double target_pw=(_d*speed_pw)+_base_pw;
pmmccorkell 0:50cdd1590925 107 double current_pw=get_pw();
pmmccorkell 0:50cdd1590925 108 double diff_pw=abs(target_pw-current_pw);
pmmccorkell 0:50cdd1590925 109 if (diff_pw>tolerance_pw) {
pmmccorkell 0:50cdd1590925 110 set_pw(target_pw);
pmmccorkell 0:50cdd1590925 111 returnval = 1;
pmmccorkell 0:50cdd1590925 112 }
pmmccorkell 0:50cdd1590925 113 }
pmmccorkell 0:50cdd1590925 114 return returnval;
pmmccorkell 0:50cdd1590925 115 }
pmmccorkell 0:50cdd1590925 116