gimbal.cpp

00001 
00002 #include <stdint.h>
00003 #include "mbed.h"
00004 #include <ros.h>
00005 #include <std_msgs/Empty.h>
00006 #include <std_msgs/Int32MultiArray.h>
00007 #include <BNO055.h>
00008 #include <initializations.h>
00009 #include <definitions.h>
00010 #include <prototypes.h>
00011 #include <Mx28.h>
00012 
00013 static int i = 0; //Variable to only update one servo each loop. 
00014 
00015 void runGimbal(void){
00016         switch(i){
00017             case(0):
00018             gimbal.servo(YAWID, control.yaw, control.yawSpeed);
00019             i++;
00020             break;
00021             case(1): 
00022             gimbal.servo(PITCHID, control.pitch, control.pitchSpeed);
00023             i++;
00024             break;
00025             case(2):
00026             gimbal.servo(ROLLID, control.roll, control.rollSpeed);
00027             i = 0;
00028             break;
00029         }                           
00030 }