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@9:30901bec3a2d, 2018-03-26 (annotated)

Committer:
MikeGray92
Date:
Mon Mar 26 21:32:30 2018 +0000
Revision:
9:30901bec3a2d
Parent:
8:478f75c6109c
Child:
10:836e701d00a6
Created 1 Chatting mode (ros_mode = 2); Added do nothing mode (ros_mode = 3)

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>
MikeGray92 7:950b3c3b5a2b 6 #include <std_msgs/Int32MultiArray.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 7 #include <prototypes.h>
MikeGray92 4:89ebfa37663b 8
MikeGray92 8:478f75c6109c 9 static int h = 0; //variable used to reset the currentPosition value when switching input direction
MikeGray92 8:478f75c6109c 10 static int initialize = 0; //Variable used to allow the set up of each mode
MikeGray92 4:89ebfa37663b 11 int rosModeDelay = 1; //The amount of delay before switching modes
group-UVic-Assistive-Technolog 0:3a767f41cf04 12
MikeGray92 7:950b3c3b5a2b 13 void module_commandCB(const std_msgs::Int32MultiArray& command_data){
group-UVic-Assistive-Technolog 0:3a767f41cf04 14 rosInput.yaw = command_data.data[ROSYAW];
group-UVic-Assistive-Technolog 0:3a767f41cf04 15 rosInput.pitch = command_data.data[ROSPITCH];
group-UVic-Assistive-Technolog 0:3a767f41cf04 16 rosInput.roll = command_data.data[ROSROLL];
group-UVic-Assistive-Technolog 0:3a767f41cf04 17 rosInput.height = command_data.data[ROSHEIGHT];
MikeGray92 4:89ebfa37663b 18 if(rosFlag == 1){
MikeGray92 4:89ebfa37663b 19 rosInput.mode = command_data.data[ROSMODE];
MikeGray92 4:89ebfa37663b 20 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 21 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 22
group-UVic-Assistive-Technolog 0:3a767f41cf04 23 void rosCheck(){
group-UVic-Assistive-Technolog 0:3a767f41cf04 24 if(rosInput.mode == 0){ //Transport Mode
MikeGray92 8:478f75c6109c 25 control.yaw = YAWZERO;
MikeGray92 8:478f75c6109c 26 control.pitch = PITCHZERO;
MikeGray92 8:478f75c6109c 27 control.roll = ROLLZERO;
MikeGray92 8:478f75c6109c 28 control.height = LIFTHEIGHTMIN;
MikeGray92 4:89ebfa37663b 29 if(initialize != rosInput.mode){
MikeGray92 4:89ebfa37663b 30 initializeMode();
MikeGray92 4:89ebfa37663b 31 }
MikeGray92 4:89ebfa37663b 32 else{
MikeGray92 4:89ebfa37663b 33 if(rosMode_Delay.read() > rosModeDelay){
MikeGray92 4:89ebfa37663b 34 rosFlag = 1;
MikeGray92 4:89ebfa37663b 35 rosMode_Delay.reset();
MikeGray92 4:89ebfa37663b 36 rosMode_Delay.stop();
MikeGray92 4:89ebfa37663b 37 }
MikeGray92 4:89ebfa37663b 38 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 39 }
MikeGray92 7:950b3c3b5a2b 40 else if(rosInput.mode == 1){ //Photo Mode
MikeGray92 2:0537a8007a39 41 if(h != rosInput.height){
MikeGray92 2:0537a8007a39 42 control.height = currentPosition;
MikeGray92 2:0537a8007a39 43 }
MikeGray92 4:89ebfa37663b 44 if(initialize != rosInput.mode){
MikeGray92 4:89ebfa37663b 45 rosFlag = 0;
MikeGray92 8:478f75c6109c 46 control.height = currentPosition;
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 9:30901bec3a2d 58 if(rosInput.mode == 2){ //Chat Mode
MikeGray92 4:89ebfa37663b 59 if(initialize != rosInput.mode){
MikeGray92 7:950b3c3b5a2b 60 control.pitch = 2122;
MikeGray92 7:950b3c3b5a2b 61 control.height = LIFTHEIGHTMAX;
MikeGray92 7:950b3c3b5a2b 62 control.yaw = YAWZERO;
MikeGray92 7:950b3c3b5a2b 63 control.roll = ROLLZERO;
MikeGray92 4:89ebfa37663b 64 initializeMode();
MikeGray92 4:89ebfa37663b 65 }
MikeGray92 4:89ebfa37663b 66 else{
MikeGray92 4:89ebfa37663b 67 rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
MikeGray92 4:89ebfa37663b 68 if(rosMode_Delay.read() > rosModeDelay){
MikeGray92 4:89ebfa37663b 69 rosFlag = 1;
MikeGray92 4:89ebfa37663b 70 rosMode_Delay.reset();
MikeGray92 4:89ebfa37663b 71 rosMode_Delay.stop();
MikeGray92 7:950b3c3b5a2b 72 }
MikeGray92 4:89ebfa37663b 73 }
MikeGray92 4:89ebfa37663b 74 }
MikeGray92 9:30901bec3a2d 75 if(rosInput.mode == 3){ //Do Nothing Mode
MikeGray92 4:89ebfa37663b 76 if(initialize != rosInput.mode){
MikeGray92 9:30901bec3a2d 77 control.height = currentPosition;
MikeGray92 4:89ebfa37663b 78 initializeMode();
MikeGray92 4:89ebfa37663b 79 }
MikeGray92 4:89ebfa37663b 80 }
MikeGray92 4:89ebfa37663b 81 }
MikeGray92 4:89ebfa37663b 82
MikeGray92 6:2ffa254e8f6e 83 // Converting the ros inputs for the lift and gimbal and determining action
MikeGray92 4:89ebfa37663b 84 void rosTranslator(int height, int yaw, int pitch, int roll){
MikeGray92 4:89ebfa37663b 85 switch(height){
MikeGray92 4:89ebfa37663b 86 case(0):
MikeGray92 6:2ffa254e8f6e 87 control.height--;
MikeGray92 6:2ffa254e8f6e 88 if(control.height < LIFTHEIGHTMIN){control.height = LIFTHEIGHTMIN;}
MikeGray92 4:89ebfa37663b 89 break;
MikeGray92 4:89ebfa37663b 90 case(1):
MikeGray92 6:2ffa254e8f6e 91 control.height = currentPosition;
MikeGray92 4:89ebfa37663b 92 break;
MikeGray92 4:89ebfa37663b 93 case(2):
MikeGray92 4:89ebfa37663b 94 control.height++;
MikeGray92 4:89ebfa37663b 95 if(control.height > LIFTHEIGHTMAX){control.height = LIFTHEIGHTMAX;}
MikeGray92 4:89ebfa37663b 96 break;
MikeGray92 4:89ebfa37663b 97 }
MikeGray92 4:89ebfa37663b 98 switch(yaw){
MikeGray92 4:89ebfa37663b 99 case(0):
MikeGray92 6:2ffa254e8f6e 100 control.yaw++;
MikeGray92 6:2ffa254e8f6e 101 if(control.yaw > YAWMAX){control.yaw = YAWMAX;}
MikeGray92 4:89ebfa37663b 102 break;
MikeGray92 4:89ebfa37663b 103 case(1):
MikeGray92 4:89ebfa37663b 104 break;
MikeGray92 4:89ebfa37663b 105 case(2):
MikeGray92 4:89ebfa37663b 106 control.yaw--;
MikeGray92 4:89ebfa37663b 107 if(control.yaw < YAWMIN){control.yaw = YAWMIN;}
MikeGray92 4:89ebfa37663b 108 break;
MikeGray92 4:89ebfa37663b 109 }
MikeGray92 4:89ebfa37663b 110 switch(pitch){
MikeGray92 4:89ebfa37663b 111 case(0):
MikeGray92 6:2ffa254e8f6e 112 control.pitch--;
MikeGray92 6:2ffa254e8f6e 113 if(control.pitch < PITCHMIN){control.pitch = PITCHMIN;}
MikeGray92 4:89ebfa37663b 114 break;
MikeGray92 4:89ebfa37663b 115 case(1):
MikeGray92 4:89ebfa37663b 116 break;
MikeGray92 4:89ebfa37663b 117 case(2):
MikeGray92 4:89ebfa37663b 118 control.pitch++;
MikeGray92 4:89ebfa37663b 119 if(control.pitch > PITCHMAX){
MikeGray92 4:89ebfa37663b 120 control.pitch = PITCHMAX;}
MikeGray92 4:89ebfa37663b 121 break;
MikeGray92 4:89ebfa37663b 122 }
MikeGray92 4:89ebfa37663b 123 switch(rosInput.roll){
MikeGray92 4:89ebfa37663b 124 case(0):
MikeGray92 6:2ffa254e8f6e 125 control.roll--;
MikeGray92 6:2ffa254e8f6e 126 if(control.roll < ROLLMIN){control.roll = ROLLMIN;}
MikeGray92 4:89ebfa37663b 127 break;
MikeGray92 4:89ebfa37663b 128 case(1):
MikeGray92 4:89ebfa37663b 129 break;
MikeGray92 4:89ebfa37663b 130 case(2):
MikeGray92 4:89ebfa37663b 131 control.roll++;
MikeGray92 4:89ebfa37663b 132 if(control.roll > ROLLMAX){control.roll = ROLLMAX;}
MikeGray92 4:89ebfa37663b 133 break;
MikeGray92 4:89ebfa37663b 134 }
MikeGray92 4:89ebfa37663b 135 }
MikeGray92 4:89ebfa37663b 136
MikeGray92 6:2ffa254e8f6e 137 //Allowing intialization in various modes
MikeGray92 4:89ebfa37663b 138 void initializeMode(){
MikeGray92 4:89ebfa37663b 139 rosFlag = 0;
MikeGray92 4:89ebfa37663b 140 if(abs(control.height - currentPosition) < 5){
MikeGray92 4:89ebfa37663b 141 initialize = rosInput.mode;
MikeGray92 4:89ebfa37663b 142 rosMode_Delay.start();
MikeGray92 4:89ebfa37663b 143 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 144 }