UVic Assistive Technology Lab
/
DSLR_Camera_Gimbal
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.
Dependencies: mbed ros_lib_kinetic
Diff: ros_functions.cpp
- Revision:
- 4:89ebfa37663b
- Parent:
- 3:527f0b949839
- Child:
- 6:2ffa254e8f6e
--- a/ros_functions.cpp Thu Feb 08 22:55:15 2018 +0000
+++ b/ros_functions.cpp Mon Mar 05 23:08:34 2018 +0000
@@ -6,98 +6,164 @@
#include <std_msgs/Float32MultiArray.h>
#include <prototypes.h>
+
int h = 0; //variable used to reset the currentPosition value when switching input direction
+int initialize = 0; //Variable used to allow the set up of each mode
+int rosModeDelay = 1; //The amount of delay before switching modes
void module_commandCB(const std_msgs::Float32MultiArray& command_data){
-
rosInput.yaw = command_data.data[ROSYAW];
rosInput.pitch = command_data.data[ROSPITCH];
rosInput.roll = command_data.data[ROSROLL];
rosInput.height = command_data.data[ROSHEIGHT];
- rosInput.mode = command_data.data[ROSMODE];
-
+ if(rosFlag == 1){
+ rosInput.mode = command_data.data[ROSMODE];
+ }
}
void rosCheck(){
if(rosInput.mode == 0){ //Transport Mode
+ if(initialize != rosInput.mode){
control.yaw = YAWZERO;
control.pitch = PITCHZERO;
control.roll = ROLLZERO;
- control.gimbalRun = TRUE;
- if(control.height != LIFTHEIGHTMIN){
control.height = LIFTHEIGHTMIN;
- control.liftRun = TRUE;
- }
+ initializeMode();
+ }
+ else{
+ if(rosMode_Delay.read() > rosModeDelay){
+ rosFlag = 1;
+ rosMode_Delay.reset();
+ rosMode_Delay.stop();
+ }
+ }
}
else if(rosInput.mode == 1){ //Photo Mode
if(h != rosInput.height){
control.height = currentPosition;
}
- switch(rosInput.height){
- case(1):
- control.height++;
- control.liftRun = TRUE;
- if(control.height > LIFTHEIGHTMAX){
- control.height = LIFTHEIGHTMAX;}
- break;
- case(2):
- control.height--;
- control.liftRun = TRUE;
- if(control.height < LIFTHEIGHTMIN){
- control.height = LIFTHEIGHTMIN;}
- break;
- case(0):
- control.height = currentPosition;
- break;
+ if(initialize != rosInput.mode){
+ rosFlag = 0;
+ initialize = rosInput.mode;
+ rosMode_Delay.start();
+ }
+ rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
+ h = rosInput.height;
+ if(rosMode_Delay.read() > rosModeDelay){
+ rosFlag = 1;
+ rosMode_Delay.reset();
+ rosMode_Delay.stop();
}
- switch(rosInput.yaw){
- case(1):
- control.yaw++;
- control.gimbalRun = TRUE;
- if(control.yaw > YAWMAX){
- control.yaw = YAWMAX;}
- break;
- case(2):
- control.yaw--;
- control.gimbalRun = TRUE;
- if(control.yaw < YAWMIN){
- control.yaw = YAWMIN;}
- break;
- case(0):
- break;
+ }
+ if(rosInput.mode == 2){ //Chat Mode - in bed
+ if(initialize != rosInput.mode){
+ control.pitch = PITCHMAX; // need to find values
+ control.height = 1700; // need to find values
+ control.yaw = 2100; // need to find values
+ initializeMode();
+ }
+ else{
+ rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
+ if(rosMode_Delay.read() > rosModeDelay){
+ rosFlag = 1;
+ rosMode_Delay.reset();
+ rosMode_Delay.stop();
+ }
+ }
+ }
+ if(rosInput.mode == 3){ //Chat Mode - Sitting
+ if(initialize != rosInput.mode){
+ control.pitch = PITCHZERO;
+ control.height = 1100; // need to find values
+ control.yaw = YAWZERO;
+ control.roll = ROLLZERO;
+ initializeMode();
+ }
+ else{
+ rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
+ if(rosMode_Delay.read() > rosModeDelay){
+ rosFlag = 1;
+ rosMode_Delay.reset();
+ rosMode_Delay.stop();
+ }
+ }
+ }
+ if(rosInput.mode == 4){ //Chat Mode - Standing
+ if(initialize != rosInput.mode){
+ control.pitch = 2122;
+ control.height = LIFTHEIGHTMAX;
+ control.yaw = YAWZERO;
+ control.roll = ROLLZERO;
+ initializeMode();
+ }
+ else{
+ rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
+ if(rosMode_Delay.read() > rosModeDelay){
+ rosFlag = 1;
+ rosMode_Delay.reset();
+ rosMode_Delay.stop();
+ }
}
- switch(rosInput.pitch){
- case(1):
- control.pitch++;
- control.gimbalRun = TRUE;
- if(control.pitch > PITCHMAX){
- control.pitch = PITCHMAX;}
- break;
- case(2):
- control.pitch--;
- control.gimbalRun = TRUE;
- if(control.pitch < PITCHMIN){
- control.pitch = PITCHMIN;}
- break;
- case(0):
- break;
- }
- switch(rosInput.roll){
- case(1):
- control.roll++;
- control.gimbalRun = TRUE;
- if(control.roll > ROLLMAX){
- control.roll = ROLLMAX;}
- break;
- case(2):
- control.roll--;
- control.gimbalRun = TRUE;
- if(control.roll < ROLLMIN){
- control.roll = ROLLMIN;}
- break;
- case(0):
- break;
- }
- h = rosInput.height;
- }
+ }
+}
+
+void rosTranslator(int height, int yaw, int pitch, int roll){
+ switch(height){
+ case(0):
+ control.height = currentPosition;
+ break;
+ case(1):
+ control.height--;
+ if(control.height < LIFTHEIGHTMIN){control.height = LIFTHEIGHTMIN;}
+ break;
+ case(2):
+ control.height++;
+ if(control.height > LIFTHEIGHTMAX){control.height = LIFTHEIGHTMAX;}
+ break;
+ }
+ switch(yaw){
+ case(0):
+ break;
+ case(1):
+ control.yaw++;
+ if(control.yaw > YAWMAX){control.yaw = YAWMAX;}
+ break;
+ case(2):
+ control.yaw--;
+ if(control.yaw < YAWMIN){control.yaw = YAWMIN;}
+ break;
+ }
+ switch(pitch){
+ case(0):
+ break;
+ case(1):
+ control.pitch--;
+ if(control.pitch < PITCHMIN){control.pitch = PITCHMIN;}
+ break;
+ case(2):
+ control.pitch++;
+ if(control.pitch > PITCHMAX){
+ control.pitch = PITCHMAX;}
+ break;
+ }
+ switch(rosInput.roll){
+ case(0):
+ break;
+ case(1):
+ control.roll--;
+ if(control.roll < ROLLMIN){control.roll = ROLLMIN;}
+ break;
+ case(2):
+ control.roll++;
+ if(control.roll > ROLLMAX){control.roll = ROLLMAX;}
+ break;
+ }
+}
+
+void initializeMode(){
+ rosFlag = 0;
+ if(abs(control.height - currentPosition) < 5){
+ initialize = rosInput.mode;
+ rosMode_Delay.start();
+ }
}
\ No newline at end of file