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@0:50cdd1590925, 2020-11-23 (annotated)

Committer:
pmmccorkell
Date:
Mon Nov 23 14:32:01 2020 +0000
Revision:
0:50cdd1590925
Child:
1:94191f7e9b27
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 0:50cdd1590925 11 #include "thruster.h"
pmmccorkell 0:50cdd1590925 12 #include "mbed.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 0:50cdd1590925 24 _max=150; // max PWM value
pmmccorkell 0:50cdd1590925 25 //pc.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 0:50cdd1590925 69 //LSB uint16_t is 10ms 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 0:50cdd1590925 80 //Accepts adjustment to max value of pw [-500,x] ms for set_speed() function.
pmmccorkell 0:50cdd1590925 81 //Returns 1 if successful.
pmmccorkell 0:50cdd1590925 82 int Thruster::set_min(double new_min) {
pmmccorkell 0:50cdd1590925 83 int returnval=0;
pmmccorkell 0:50cdd1590925 84 if (new_min>-500) {
pmmccorkell 0:50cdd1590925 85 _min=new_min;
pmmccorkell 0:50cdd1590925 86 returnval=1;
pmmccorkell 0:50cdd1590925 87 }
pmmccorkell 0:50cdd1590925 88 return returnval;
pmmccorkell 0:50cdd1590925 89 }
pmmccorkell 0:50cdd1590925 90
pmmccorkell 0:50cdd1590925 91 //Accepts adjustment to max value of pw [x,500] ms for set_speed() function.
pmmccorkell 0:50cdd1590925 92 //Returns 1 if successful.
pmmccorkell 0:50cdd1590925 93 int Thruster::set_max(double new_max) {
pmmccorkell 0:50cdd1590925 94 int returnval=0;
pmmccorkell 0:50cdd1590925 95 if (new_max<500) {
pmmccorkell 0:50cdd1590925 96 _max=new_max;
pmmccorkell 0:50cdd1590925 97 returnval=1;
pmmccorkell 0:50cdd1590925 98 }
pmmccorkell 0:50cdd1590925 99 return returnval;
pmmccorkell 0:50cdd1590925 100 }
pmmccorkell 0:50cdd1590925 101
pmmccorkell 0:50cdd1590925 102 //Accepts adjustment to pw [-500,500] ms that is added to 1.5ms.
pmmccorkell 0:50cdd1590925 103 //Returns 1 if successful.
pmmccorkell 0:50cdd1590925 104 int Thruster::set_speed(double speed_pw) {
pmmccorkell 0:50cdd1590925 105 int returnval = 0;
pmmccorkell 0:50cdd1590925 106 if (_lock==1) {
pmmccorkell 0:50cdd1590925 107 set_pw(_base_pw);
pmmccorkell 0:50cdd1590925 108 }
pmmccorkell 0:50cdd1590925 109 else if (abs(speed_pw) > _max) {
pmmccorkell 0:50cdd1590925 110 returnval = 0;
pmmccorkell 0:50cdd1590925 111 }
pmmccorkell 0:50cdd1590925 112 else {
pmmccorkell 0:50cdd1590925 113 double tolerance_pw=0.001;
pmmccorkell 0:50cdd1590925 114 double target_pw=(_d*speed_pw)+_base_pw;
pmmccorkell 0:50cdd1590925 115 double current_pw=get_pw();
pmmccorkell 0:50cdd1590925 116 double diff_pw=abs(target_pw-current_pw);
pmmccorkell 0:50cdd1590925 117 if (diff_pw>tolerance_pw) {
pmmccorkell 0:50cdd1590925 118 set_pw(target_pw);
pmmccorkell 0:50cdd1590925 119 returnval = 1;
pmmccorkell 0:50cdd1590925 120 }
pmmccorkell 0:50cdd1590925 121 }
pmmccorkell 0:50cdd1590925 122 return returnval;
pmmccorkell 0:50cdd1590925 123 }
pmmccorkell 0:50cdd1590925 124