USNA ES456 Autonomous Vehicles / Mbed 2 deprecated MadPulse_Controller_ros

Madpulse ROS Code

Dependencies:   mbed ServoOut BNO055_fusion ros_lib_kinetic MadPulseIMU ServoIn

RC/RC_Channel.cpp@5:c24490c61022, 2019-09-12 (annotated)

Committer:
jdawkins
Date:
Thu Sep 12 13:40:43 2019 +0000
Revision:
5:c24490c61022
Initial Commit of Madpulse Ros Code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jdawkins 5:c24490c61022 1 #include "RC_Channel.h"
jdawkins 5:c24490c61022 2 #include "mbed.h"
jdawkins 5:c24490c61022 3
jdawkins 5:c24490c61022 4 RC_Channel::RC_Channel(PinName mypin, int index) : myinterrupt(mypin)
jdawkins 5:c24490c61022 5 {
jdawkins 5:c24490c61022 6 LocalFileSystem local("local"); // If theres no local yet
jdawkins 5:c24490c61022 7 RC_Channel::index = index;
jdawkins 5:c24490c61022 8 time = -100; // start value to see if there was any value yet
jdawkins 5:c24490c61022 9
jdawkins 5:c24490c61022 10 loadCalibrationValue(&scale, "SCALE");
jdawkins 5:c24490c61022 11 loadCalibrationValue(&offset, "OFFSE");
jdawkins 5:c24490c61022 12
jdawkins 5:c24490c61022 13 myinterrupt.rise(this, &RC_Channel::rise);
jdawkins 5:c24490c61022 14 myinterrupt.fall(this, &RC_Channel::fall);
jdawkins 5:c24490c61022 15 timeoutchecker.attach(this, &RC_Channel::timeoutcheck, 1);
jdawkins 5:c24490c61022 16 }
jdawkins 5:c24490c61022 17
jdawkins 5:c24490c61022 18 int RC_Channel::read()
jdawkins 5:c24490c61022 19 {
jdawkins 5:c24490c61022 20 if(time == -100)
jdawkins 5:c24490c61022 21 return time;
jdawkins 5:c24490c61022 22 float result = scale * (float)(time) + offset; // calibration of the readings
jdawkins 5:c24490c61022 23 return (result > 490 && result < 510) ? 500 : result; // make the middle of the stickpositions non drifting
jdawkins 5:c24490c61022 24 }
jdawkins 5:c24490c61022 25
jdawkins 5:c24490c61022 26 void RC_Channel::rise()
jdawkins 5:c24490c61022 27 {
jdawkins 5:c24490c61022 28 timer.start();
jdawkins 5:c24490c61022 29 }
jdawkins 5:c24490c61022 30
jdawkins 5:c24490c61022 31 void RC_Channel::fall()
jdawkins 5:c24490c61022 32 {
jdawkins 5:c24490c61022 33 timer.stop();
jdawkins 5:c24490c61022 34 int tester = timer.read_us();
jdawkins 5:c24490c61022 35 if(tester >= 1000 && tester <=2000)
jdawkins 5:c24490c61022 36 time = tester-1000; // we want only the signal from 1000 - 2000 as 0 - 1000 for easier scaling
jdawkins 5:c24490c61022 37 timer.reset();
jdawkins 5:c24490c61022 38 timer.start();
jdawkins 5:c24490c61022 39 }
jdawkins 5:c24490c61022 40
jdawkins 5:c24490c61022 41 void RC_Channel::timeoutcheck()
jdawkins 5:c24490c61022 42 {
jdawkins 5:c24490c61022 43 if (timer.read() > 0.3)
jdawkins 5:c24490c61022 44 time = -100;
jdawkins 5:c24490c61022 45 }
jdawkins 5:c24490c61022 46
jdawkins 5:c24490c61022 47 void RC_Channel::saveCalibrationValue(float * value, char * fileextension)
jdawkins 5:c24490c61022 48 {
jdawkins 5:c24490c61022 49 char path[40];
jdawkins 5:c24490c61022 50 sprintf(path, "/local/FlyBed/RC_%d_%s", index, fileextension);
jdawkins 5:c24490c61022 51 FILE *fp = fopen(path, "w");
jdawkins 5:c24490c61022 52 if (fp != NULL) {
jdawkins 5:c24490c61022 53 fprintf(fp, "%f", value);
jdawkins 5:c24490c61022 54 fclose(fp);
jdawkins 5:c24490c61022 55 } else
jdawkins 5:c24490c61022 56 value = 0;
jdawkins 5:c24490c61022 57 }
jdawkins 5:c24490c61022 58
jdawkins 5:c24490c61022 59 void RC_Channel::loadCalibrationValue(float * value, char * fileextension)
jdawkins 5:c24490c61022 60 {
jdawkins 5:c24490c61022 61 char path[40];
jdawkins 5:c24490c61022 62 sprintf(path, "/local/RC%d%s", index, fileextension);
jdawkins 5:c24490c61022 63 FILE *fp = fopen(path, "r");
jdawkins 5:c24490c61022 64 if (fp != NULL) {
jdawkins 5:c24490c61022 65 fscanf(fp, "%f", value);
jdawkins 5:c24490c61022 66 fclose(fp);
jdawkins 5:c24490c61022 67 } else
jdawkins 5:c24490c61022 68 value = 0;
jdawkins 5:c24490c61022 69 }