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@14:97804177806d, 2018-12-14 (annotated)

Committer:
MikeGray92
Date:
Fri Dec 14 22:05:56 2018 +0000
Revision:
14:97804177806d
Parent:
13:8630f38f8066 
fixed compiling errors

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