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@11:f99511d770ed, 2018-05-10 (annotated)

Committer:
MikeGray92
Date:
Thu May 10 00:57:32 2018 +0000
Revision:
11:f99511d770ed
Parent:
10:836e701d00a6
Child:
13:8630f38f8066
yup

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];
MikeGray92 10:836e701d00a6 17 rosInput.yawSpeed = command_data.data[ROSYAWSP];
group-UVic-Assistive-Technolog 0:3a767f41cf04 18 rosInput.height = command_data.data[ROSHEIGHT];
MikeGray92 10:836e701d00a6 19 rosInput.rollSpeed = command_data.data[ROSROLLSP];
MikeGray92 10:836e701d00a6 20 rosInput.pitchSpeed = command_data.data[ROSPITCHSP];
MikeGray92 10:836e701d00a6 21 rosInput.heightSpeed = command_data.data[ROSHEIGHTSP];
MikeGray92 4:89ebfa37663b 22 if(rosFlag == 1){
MikeGray92 4:89ebfa37663b 23 rosInput.mode = command_data.data[ROSMODE];
MikeGray92 4:89ebfa37663b 24 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 25 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 26
group-UVic-Assistive-Technolog 0:3a767f41cf04 27 void rosCheck(){
group-UVic-Assistive-Technolog 0:3a767f41cf04 28 if(rosInput.mode == 0){ //Transport Mode
MikeGray92 8:478f75c6109c 29 control.yaw = YAWZERO;
MikeGray92 8:478f75c6109c 30 control.pitch = PITCHZERO;
MikeGray92 8:478f75c6109c 31 control.roll = ROLLZERO;
MikeGray92 8:478f75c6109c 32 control.height = LIFTHEIGHTMIN;
MikeGray92 10:836e701d00a6 33 control.heightSpeed = 0;
MikeGray92 4:89ebfa37663b 34 if(initialize != rosInput.mode){
MikeGray92 4:89ebfa37663b 35 initializeMode();
MikeGray92 4:89ebfa37663b 36 }
MikeGray92 4:89ebfa37663b 37 else{
MikeGray92 4:89ebfa37663b 38 if(rosMode_Delay.read() > rosModeDelay){
MikeGray92 4:89ebfa37663b 39 rosFlag = 1;
MikeGray92 4:89ebfa37663b 40 rosMode_Delay.reset();
MikeGray92 4:89ebfa37663b 41 rosMode_Delay.stop();
MikeGray92 4:89ebfa37663b 42 }
MikeGray92 4:89ebfa37663b 43 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 44 }
MikeGray92 7:950b3c3b5a2b 45 else if(rosInput.mode == 1){ //Photo Mode
MikeGray92 2:0537a8007a39 46 if(h != rosInput.height){
MikeGray92 2:0537a8007a39 47 control.height = currentPosition;
MikeGray92 2:0537a8007a39 48 }
MikeGray92 4:89ebfa37663b 49 if(initialize != rosInput.mode){
MikeGray92 4:89ebfa37663b 50 rosFlag = 0;
MikeGray92 8:478f75c6109c 51 control.height = currentPosition;
MikeGray92 4:89ebfa37663b 52 initialize = rosInput.mode;
MikeGray92 4:89ebfa37663b 53 rosMode_Delay.start();
MikeGray92 4:89ebfa37663b 54 }
MikeGray92 4:89ebfa37663b 55 rosTranslator(rosInput.height, rosInput.yaw, rosInput.pitch, rosInput.roll);
MikeGray92 4:89ebfa37663b 56 h = rosInput.height;
MikeGray92 4:89ebfa37663b 57 if(rosMode_Delay.read() > rosModeDelay){
MikeGray92 4:89ebfa37663b 58 rosFlag = 1;
MikeGray92 4:89ebfa37663b 59 rosMode_Delay.reset();
MikeGray92 4:89ebfa37663b 60 rosMode_Delay.stop();
group-UVic-Assistive-Technolog 0:3a767f41cf04 61 }
MikeGray92 4:89ebfa37663b 62 }
MikeGray92 4:89ebfa37663b 63 }
MikeGray92 4:89ebfa37663b 64
MikeGray92 6:2ffa254e8f6e 65 // Converting the ros inputs for the lift and gimbal and determining action
MikeGray92 4:89ebfa37663b 66 void rosTranslator(int height, int yaw, int pitch, int roll){
MikeGray92 10:836e701d00a6 67 if(eStopFlag == 0){
MikeGray92 10:836e701d00a6 68 //Updating Height Position
MikeGray92 10:836e701d00a6 69 if(rosInput.height < LIFTHEIGHTMIN){control.height = LIFTHEIGHTMIN;}
MikeGray92 10:836e701d00a6 70 else if(rosInput.height > LIFTHEIGHTMAX){control.height = LIFTHEIGHTMAX;}
MikeGray92 10:836e701d00a6 71 else {control.height = rosInput.height;}
MikeGray92 10:836e701d00a6 72 //Updating Height Speed
MikeGray92 10:836e701d00a6 73 switch(rosInput.heightSpeed){
MikeGray92 10:836e701d00a6 74 case(0):
MikeGray92 10:836e701d00a6 75 liftDutyCycle = 0;
MikeGray92 10:836e701d00a6 76 break;
MikeGray92 10:836e701d00a6 77 case(1):
MikeGray92 10:836e701d00a6 78 liftDutyCycle = 0.1;
MikeGray92 10:836e701d00a6 79 break;
MikeGray92 10:836e701d00a6 80 case(2):
MikeGray92 10:836e701d00a6 81 liftDutyCycle = 0.2;
MikeGray92 10:836e701d00a6 82 break;
MikeGray92 10:836e701d00a6 83 case(3):
MikeGray92 10:836e701d00a6 84 liftDutyCycle = 0.3;
MikeGray92 10:836e701d00a6 85 break;
MikeGray92 10:836e701d00a6 86 case(4):
MikeGray92 10:836e701d00a6 87 liftDutyCycle = 0.4;
MikeGray92 10:836e701d00a6 88 break;
MikeGray92 10:836e701d00a6 89 case(5):
MikeGray92 10:836e701d00a6 90 liftDutyCycle = 0.5;
MikeGray92 10:836e701d00a6 91 break;
MikeGray92 10:836e701d00a6 92 case(6):
MikeGray92 10:836e701d00a6 93 liftDutyCycle = 0.6;
MikeGray92 10:836e701d00a6 94 break;
MikeGray92 10:836e701d00a6 95 case(7):
MikeGray92 10:836e701d00a6 96 liftDutyCycle = 0.7;
MikeGray92 10:836e701d00a6 97 break;
MikeGray92 10:836e701d00a6 98 case(8):
MikeGray92 10:836e701d00a6 99 liftDutyCycle = 0.8;
MikeGray92 10:836e701d00a6 100 break;
MikeGray92 10:836e701d00a6 101 case(9):
MikeGray92 10:836e701d00a6 102 liftDutyCycle = 0.9;
MikeGray92 10:836e701d00a6 103 break;
MikeGray92 10:836e701d00a6 104 case(10):
MikeGray92 10:836e701d00a6 105 liftDutyCycle = 1;
MikeGray92 10:836e701d00a6 106 break;
MikeGray92 10:836e701d00a6 107 }
MikeGray92 10:836e701d00a6 108
MikeGray92 10:836e701d00a6 109 //Updating Yaw Position
MikeGray92 11:f99511d770ed 110 if(rosInput.pitchSpeed == 0){}
MikeGray92 11:f99511d770ed 111 else {
MikeGray92 11:f99511d770ed 112 if(rosInput.yaw < YAWMIN){control.yaw = YAWMIN;}
MikeGray92 11:f99511d770ed 113 else if(rosInput.yaw > YAWMAX){control.yaw = YAWMAX;}
MikeGray92 11:f99511d770ed 114 else {control.yaw = rosInput.yaw;}
MikeGray92 11:f99511d770ed 115 }
MikeGray92 10:836e701d00a6 116 //Updating Roll Speed
MikeGray92 10:836e701d00a6 117 if(rosInput.yawSpeed > SERVOSPEEDMAX){control.yawSpeed = SERVOSPEEDMAX;}
MikeGray92 10:836e701d00a6 118 else if(rosInput.yawSpeed < SERVOSPEEDMIN){control.yawSpeed = SERVOSPEEDMIN;}
MikeGray92 10:836e701d00a6 119 else {control.yawSpeed = rosInput.yawSpeed;}
MikeGray92 10:836e701d00a6 120
MikeGray92 10:836e701d00a6 121 //Updating Pitch Position
MikeGray92 11:f99511d770ed 122 if(rosInput.pitchSpeed == 0){}
MikeGray92 11:f99511d770ed 123 else {
MikeGray92 11:f99511d770ed 124 if(rosInput.pitch < PITCHMIN){control.pitch = PITCHMIN;}
MikeGray92 11:f99511d770ed 125 else if(rosInput.pitch > PITCHMAX){control.pitch = PITCHMAX;}
MikeGray92 11:f99511d770ed 126 else {control.pitch = rosInput.pitch;}
MikeGray92 11:f99511d770ed 127 }
MikeGray92 10:836e701d00a6 128 //Updating Pitch Speed
MikeGray92 10:836e701d00a6 129 if(rosInput.pitchSpeed > SERVOSPEEDMAX){control.pitchSpeed = SERVOSPEEDMAX;}
MikeGray92 10:836e701d00a6 130 else if(rosInput.pitchSpeed < SERVOSPEEDMIN){control.pitchSpeed = SERVOSPEEDMIN;}
MikeGray92 10:836e701d00a6 131 else {control.pitchSpeed = rosInput.pitchSpeed;}
MikeGray92 10:836e701d00a6 132
MikeGray92 10:836e701d00a6 133 //Updating Roll Position
MikeGray92 11:f99511d770ed 134 if(rosInput.rollSpeed == 0){}
MikeGray92 11:f99511d770ed 135 else {
MikeGray92 11:f99511d770ed 136 if(rosInput.roll < ROLLMIN){control.roll = ROLLMIN;}
MikeGray92 11:f99511d770ed 137 else if(rosInput.roll > ROLLMAX){control.roll = ROLLMAX;}
MikeGray92 11:f99511d770ed 138 else {control.roll = rosInput.roll;}
MikeGray92 11:f99511d770ed 139 }
MikeGray92 10:836e701d00a6 140 //Updating Roll Speed
MikeGray92 10:836e701d00a6 141 if(rosInput.rollSpeed > SERVOSPEEDMAX){control.rollSpeed = SERVOSPEEDMAX;}
MikeGray92 10:836e701d00a6 142 else if(rosInput.rollSpeed < SERVOSPEEDMIN){control.rollSpeed = SERVOSPEEDMIN;}
MikeGray92 10:836e701d00a6 143 else {control.rollSpeed = rosInput.rollSpeed;}
MikeGray92 10:836e701d00a6 144
MikeGray92 4:89ebfa37663b 145 }
MikeGray92 4:89ebfa37663b 146 }
MikeGray92 4:89ebfa37663b 147
MikeGray92 6:2ffa254e8f6e 148 //Allowing intialization in various modes
MikeGray92 4:89ebfa37663b 149 void initializeMode(){
MikeGray92 4:89ebfa37663b 150 rosFlag = 0;
MikeGray92 4:89ebfa37663b 151 if(abs(control.height - currentPosition) < 5){
MikeGray92 4:89ebfa37663b 152 initialize = rosInput.mode;
MikeGray92 4:89ebfa37663b 153 rosMode_Delay.start();
MikeGray92 4:89ebfa37663b 154 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 155 }