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@10:836e701d00a6, 2018-04-26 (annotated)

Committer:
MikeGray92
Date:
Thu Apr 26 18:33:11 2018 +0000
Revision:
10:836e701d00a6
Parent:
9:30901bec3a2d
Child:
11:f99511d770ed
Speed and Position controlled lift and 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>
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 10:836e701d00a6 110 if(rosInput.yaw < YAWMIN){control.yaw = YAWMIN;}
MikeGray92 10:836e701d00a6 111 else if(rosInput.yaw > YAWMAX){control.yaw = YAWMAX;}
MikeGray92 10:836e701d00a6 112 else {control.yaw = rosInput.yaw;}
MikeGray92 10:836e701d00a6 113 //Updating Roll Speed
MikeGray92 10:836e701d00a6 114 if(rosInput.yawSpeed > SERVOSPEEDMAX){control.yawSpeed = SERVOSPEEDMAX;}
MikeGray92 10:836e701d00a6 115 else if(rosInput.yawSpeed < SERVOSPEEDMIN){control.yawSpeed = SERVOSPEEDMIN;}
MikeGray92 10:836e701d00a6 116 else {control.yawSpeed = rosInput.yawSpeed;}
MikeGray92 10:836e701d00a6 117
MikeGray92 10:836e701d00a6 118 //Updating Pitch Position
MikeGray92 10:836e701d00a6 119 if(rosInput.pitch < PITCHMIN){control.pitch = PITCHMIN;}
MikeGray92 10:836e701d00a6 120 else if(rosInput.pitch > PITCHMAX){control.pitch = PITCHMAX;}
MikeGray92 10:836e701d00a6 121 else {control.pitch = rosInput.pitch;}
MikeGray92 10:836e701d00a6 122 //Updating Pitch Speed
MikeGray92 10:836e701d00a6 123 if(rosInput.pitchSpeed > SERVOSPEEDMAX){control.pitchSpeed = SERVOSPEEDMAX;}
MikeGray92 10:836e701d00a6 124 else if(rosInput.pitchSpeed < SERVOSPEEDMIN){control.pitchSpeed = SERVOSPEEDMIN;}
MikeGray92 10:836e701d00a6 125 else {control.pitchSpeed = rosInput.pitchSpeed;}
MikeGray92 10:836e701d00a6 126
MikeGray92 10:836e701d00a6 127 //Updating Roll Position
MikeGray92 10:836e701d00a6 128 if(rosInput.roll < ROLLMIN){control.roll = ROLLMIN;}
MikeGray92 10:836e701d00a6 129 else if(rosInput.roll > ROLLMAX){control.roll = ROLLMAX;}
MikeGray92 10:836e701d00a6 130 else {control.roll = rosInput.roll;}
MikeGray92 10:836e701d00a6 131 //Updating Roll Speed
MikeGray92 10:836e701d00a6 132 if(rosInput.rollSpeed > SERVOSPEEDMAX){control.rollSpeed = SERVOSPEEDMAX;}
MikeGray92 10:836e701d00a6 133 else if(rosInput.rollSpeed < SERVOSPEEDMIN){control.rollSpeed = SERVOSPEEDMIN;}
MikeGray92 10:836e701d00a6 134 else {control.rollSpeed = rosInput.rollSpeed;}
MikeGray92 10:836e701d00a6 135
MikeGray92 4:89ebfa37663b 136 }
MikeGray92 4:89ebfa37663b 137 }
MikeGray92 4:89ebfa37663b 138
MikeGray92 6:2ffa254e8f6e 139 //Allowing intialization in various modes
MikeGray92 4:89ebfa37663b 140 void initializeMode(){
MikeGray92 4:89ebfa37663b 141 rosFlag = 0;
MikeGray92 4:89ebfa37663b 142 if(abs(control.height - currentPosition) < 5){
MikeGray92 4:89ebfa37663b 143 initialize = rosInput.mode;
MikeGray92 4:89ebfa37663b 144 rosMode_Delay.start();
MikeGray92 4:89ebfa37663b 145 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 146 }