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@3:02a9d402226d, 2020-11-24 (annotated)

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