UVic Assistive Technology Lab / Mbed 2 deprecated 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

ros_functions.cpp@4:89ebfa37663b, 2018-03-05 (annotated)

Committer:
MikeGray92
Date:
Mon Mar 05 23:08:34 2018 +0000
Revision:
4:89ebfa37663b
Parent:
3:527f0b949839
Child:
6:2ffa254e8f6e
Added chat modes; Defined new limits for gimbal

Who changed what in which revision?

UserRevisionLine numberNew contents of line
group-UVic-Assistive-Technolog 0:3a767f41cf04 1
group-UVic-Assistive-Technolog 0:3a767f41cf04 2 #include <ros_functions.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 3 #include <definitions.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 4 #include <ros.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 5 #include <std_msgs/Empty.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 6 #include <std_msgs/Float32MultiArray.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 7 #include <prototypes.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 8
MikeGray92 4:89ebfa37663b 9
MikeGray92 3:527f0b949839 10 int h = 0; //variable used to reset the currentPosition value when switching input direction
MikeGray92 4:89ebfa37663b 11 int initialize = 0; //Variable used to allow the set up of each mode
MikeGray92 4:89ebfa37663b 12 int rosModeDelay = 1; //The amount of delay before switching modes
group-UVic-Assistive-Technolog 0:3a767f41cf04 13
group-UVic-Assistive-Technolog 0:3a767f41cf04 14 void module_commandCB(const std_msgs::Float32MultiArray& command_data){
group-UVic-Assistive-Technolog 0:3a767f41cf04 15 rosInput.yaw = command_data.data[ROSYAW];
group-UVic-Assistive-Technolog 0:3a767f41cf04 16 rosInput.pitch = command_data.data[ROSPITCH];
group-UVic-Assistive-Technolog 0:3a767f41cf04 17 rosInput.roll = command_data.data[ROSROLL];
group-UVic-Assistive-Technolog 0:3a767f41cf04 18 rosInput.height = command_data.data[ROSHEIGHT];
MikeGray92 4:89ebfa37663b 19 if(rosFlag == 1){
MikeGray92 4:89ebfa37663b 20 rosInput.mode = command_data.data[ROSMODE];
MikeGray92 4:89ebfa37663b 21 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 22 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 23
group-UVic-Assistive-Technolog 0:3a767f41cf04 24 void rosCheck(){
group-UVic-Assistive-Technolog 0:3a767f41cf04 25 if(rosInput.mode == 0){ //Transport Mode
MikeGray92 4:89ebfa37663b 26 if(initialize != rosInput.mode){
group-UVic-Assistive-Technolog 0:3a767f41cf04 27 control.yaw = YAWZERO;
group-UVic-Assistive-Technolog 0:3a767f41cf04 28 control.pitch = PITCHZERO;
group-UVic-Assistive-Technolog 0:3a767f41cf04 29 control.roll = ROLLZERO;
group-UVic-Assistive-Technolog 0:3a767f41cf04 30 control.height = LIFTHEIGHTMIN;
MikeGray92 4:89ebfa37663b 31 initializeMode();
MikeGray92 4:89ebfa37663b 32 }
MikeGray92 4:89ebfa37663b 33 else{
MikeGray92 4:89ebfa37663b 34 if(rosMode_Delay.read() > rosModeDelay){
MikeGray92 4:89ebfa37663b 35 rosFlag = 1;
MikeGray92 4:89ebfa37663b 36 rosMode_Delay.reset();
MikeGray92 4:89ebfa37663b 37 rosMode_Delay.stop();
MikeGray92 4:89ebfa37663b 38 }
MikeGray92 4:89ebfa37663b 39 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 40 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 41 else if(rosInput.mode == 1){ //Photo Mode
MikeGray92 2:0537a8007a39 42 if(h != rosInput.height){
MikeGray92 2:0537a8007a39 43 control.height = currentPosition;
MikeGray92 2:0537a8007a39 44 }
MikeGray92 4:89ebfa37663b 45 if(initialize != rosInput.mode){
MikeGray92 4:89ebfa37663b 46 rosFlag = 0;
MikeGray92 4:89ebfa37663b 47 initialize = rosInput.mode;
MikeGray92 4:89ebfa37663b 48 rosMode_Delay.start();
MikeGray92 4:89ebfa37663b 49 }
MikeGray92 4:89ebfa37663b 50 rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
MikeGray92 4:89ebfa37663b 51 h = rosInput.height;
MikeGray92 4:89ebfa37663b 52 if(rosMode_Delay.read() > rosModeDelay){
MikeGray92 4:89ebfa37663b 53 rosFlag = 1;
MikeGray92 4:89ebfa37663b 54 rosMode_Delay.reset();
MikeGray92 4:89ebfa37663b 55 rosMode_Delay.stop();
group-UVic-Assistive-Technolog 0:3a767f41cf04 56 }
MikeGray92 4:89ebfa37663b 57 }
MikeGray92 4:89ebfa37663b 58 if(rosInput.mode == 2){ //Chat Mode - in bed
MikeGray92 4:89ebfa37663b 59 if(initialize != rosInput.mode){
MikeGray92 4:89ebfa37663b 60 control.pitch = PITCHMAX; // need to find values
MikeGray92 4:89ebfa37663b 61 control.height = 1700; // need to find values
MikeGray92 4:89ebfa37663b 62 control.yaw = 2100; // need to find values
MikeGray92 4:89ebfa37663b 63 initializeMode();
MikeGray92 4:89ebfa37663b 64 }
MikeGray92 4:89ebfa37663b 65 else{
MikeGray92 4:89ebfa37663b 66 rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
MikeGray92 4:89ebfa37663b 67 if(rosMode_Delay.read() > rosModeDelay){
MikeGray92 4:89ebfa37663b 68 rosFlag = 1;
MikeGray92 4:89ebfa37663b 69 rosMode_Delay.reset();
MikeGray92 4:89ebfa37663b 70 rosMode_Delay.stop();
MikeGray92 4:89ebfa37663b 71 }
MikeGray92 4:89ebfa37663b 72 }
MikeGray92 4:89ebfa37663b 73 }
MikeGray92 4:89ebfa37663b 74 if(rosInput.mode == 3){ //Chat Mode - Sitting
MikeGray92 4:89ebfa37663b 75 if(initialize != rosInput.mode){
MikeGray92 4:89ebfa37663b 76 control.pitch = PITCHZERO;
MikeGray92 4:89ebfa37663b 77 control.height = 1100; // need to find values
MikeGray92 4:89ebfa37663b 78 control.yaw = YAWZERO;
MikeGray92 4:89ebfa37663b 79 control.roll = ROLLZERO;
MikeGray92 4:89ebfa37663b 80 initializeMode();
MikeGray92 4:89ebfa37663b 81 }
MikeGray92 4:89ebfa37663b 82 else{
MikeGray92 4:89ebfa37663b 83 rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
MikeGray92 4:89ebfa37663b 84 if(rosMode_Delay.read() > rosModeDelay){
MikeGray92 4:89ebfa37663b 85 rosFlag = 1;
MikeGray92 4:89ebfa37663b 86 rosMode_Delay.reset();
MikeGray92 4:89ebfa37663b 87 rosMode_Delay.stop();
MikeGray92 4:89ebfa37663b 88 }
MikeGray92 4:89ebfa37663b 89 }
MikeGray92 4:89ebfa37663b 90 }
MikeGray92 4:89ebfa37663b 91 if(rosInput.mode == 4){ //Chat Mode - Standing
MikeGray92 4:89ebfa37663b 92 if(initialize != rosInput.mode){
MikeGray92 4:89ebfa37663b 93 control.pitch = 2122;
MikeGray92 4:89ebfa37663b 94 control.height = LIFTHEIGHTMAX;
MikeGray92 4:89ebfa37663b 95 control.yaw = YAWZERO;
MikeGray92 4:89ebfa37663b 96 control.roll = ROLLZERO;
MikeGray92 4:89ebfa37663b 97 initializeMode();
MikeGray92 4:89ebfa37663b 98 }
MikeGray92 4:89ebfa37663b 99 else{
MikeGray92 4:89ebfa37663b 100 rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
MikeGray92 4:89ebfa37663b 101 if(rosMode_Delay.read() > rosModeDelay){
MikeGray92 4:89ebfa37663b 102 rosFlag = 1;
MikeGray92 4:89ebfa37663b 103 rosMode_Delay.reset();
MikeGray92 4:89ebfa37663b 104 rosMode_Delay.stop();
MikeGray92 4:89ebfa37663b 105 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 106 }
MikeGray92 4:89ebfa37663b 107 }
MikeGray92 4:89ebfa37663b 108 }
MikeGray92 4:89ebfa37663b 109
MikeGray92 4:89ebfa37663b 110 void rosTranslator(int height, int yaw, int pitch, int roll){
MikeGray92 4:89ebfa37663b 111 switch(height){
MikeGray92 4:89ebfa37663b 112 case(0):
MikeGray92 4:89ebfa37663b 113 control.height = currentPosition;
MikeGray92 4:89ebfa37663b 114 break;
MikeGray92 4:89ebfa37663b 115 case(1):
MikeGray92 4:89ebfa37663b 116 control.height--;
MikeGray92 4:89ebfa37663b 117 if(control.height < LIFTHEIGHTMIN){control.height = LIFTHEIGHTMIN;}
MikeGray92 4:89ebfa37663b 118 break;
MikeGray92 4:89ebfa37663b 119 case(2):
MikeGray92 4:89ebfa37663b 120 control.height++;
MikeGray92 4:89ebfa37663b 121 if(control.height > LIFTHEIGHTMAX){control.height = LIFTHEIGHTMAX;}
MikeGray92 4:89ebfa37663b 122 break;
MikeGray92 4:89ebfa37663b 123 }
MikeGray92 4:89ebfa37663b 124 switch(yaw){
MikeGray92 4:89ebfa37663b 125 case(0):
MikeGray92 4:89ebfa37663b 126 break;
MikeGray92 4:89ebfa37663b 127 case(1):
MikeGray92 4:89ebfa37663b 128 control.yaw++;
MikeGray92 4:89ebfa37663b 129 if(control.yaw > YAWMAX){control.yaw = YAWMAX;}
MikeGray92 4:89ebfa37663b 130 break;
MikeGray92 4:89ebfa37663b 131 case(2):
MikeGray92 4:89ebfa37663b 132 control.yaw--;
MikeGray92 4:89ebfa37663b 133 if(control.yaw < YAWMIN){control.yaw = YAWMIN;}
MikeGray92 4:89ebfa37663b 134 break;
MikeGray92 4:89ebfa37663b 135 }
MikeGray92 4:89ebfa37663b 136 switch(pitch){
MikeGray92 4:89ebfa37663b 137 case(0):
MikeGray92 4:89ebfa37663b 138 break;
MikeGray92 4:89ebfa37663b 139 case(1):
MikeGray92 4:89ebfa37663b 140 control.pitch--;
MikeGray92 4:89ebfa37663b 141 if(control.pitch < PITCHMIN){control.pitch = PITCHMIN;}
MikeGray92 4:89ebfa37663b 142 break;
MikeGray92 4:89ebfa37663b 143 case(2):
MikeGray92 4:89ebfa37663b 144 control.pitch++;
MikeGray92 4:89ebfa37663b 145 if(control.pitch > PITCHMAX){
MikeGray92 4:89ebfa37663b 146 control.pitch = PITCHMAX;}
MikeGray92 4:89ebfa37663b 147 break;
MikeGray92 4:89ebfa37663b 148 }
MikeGray92 4:89ebfa37663b 149 switch(rosInput.roll){
MikeGray92 4:89ebfa37663b 150 case(0):
MikeGray92 4:89ebfa37663b 151 break;
MikeGray92 4:89ebfa37663b 152 case(1):
MikeGray92 4:89ebfa37663b 153 control.roll--;
MikeGray92 4:89ebfa37663b 154 if(control.roll < ROLLMIN){control.roll = ROLLMIN;}
MikeGray92 4:89ebfa37663b 155 break;
MikeGray92 4:89ebfa37663b 156 case(2):
MikeGray92 4:89ebfa37663b 157 control.roll++;
MikeGray92 4:89ebfa37663b 158 if(control.roll > ROLLMAX){control.roll = ROLLMAX;}
MikeGray92 4:89ebfa37663b 159 break;
MikeGray92 4:89ebfa37663b 160 }
MikeGray92 4:89ebfa37663b 161 }
MikeGray92 4:89ebfa37663b 162
MikeGray92 4:89ebfa37663b 163 void initializeMode(){
MikeGray92 4:89ebfa37663b 164 rosFlag = 0;
MikeGray92 4:89ebfa37663b 165 if(abs(control.height - currentPosition) < 5){
MikeGray92 4:89ebfa37663b 166 initialize = rosInput.mode;
MikeGray92 4:89ebfa37663b 167 rosMode_Delay.start();
MikeGray92 4:89ebfa37663b 168 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 169 }