int main(void) {
// Configure the oscillator
initOsc();
// Initialize I/O and peripherals
initPorts();
// Set starting conditions
PORTB = 0b00010000; //prepare for 5 seconds delay
while (S6 == 1); //wait till S6 is pressed
doFive(); //do 5 seconds delay
LED12 = 1;
for (int i = 12; i > 0; i--) {//Jiggle back and forth to unfold our UberRamps
PORTB = 0b00001001;
__delay_ms(100);
PORTB = 0b00000110;
__delay_ms(100);
}
__delay_ms(1000);
while (true) {
snsfwd(); //call to do the main code forever
}
}
void addOsc(MidiMap midi, int pin) {
Osc *osc = new Osc;
initOsc(osc, pin);
addObject(midi, osc, (tickFn)tickOsc, (noteOnFn) playOsc, (noteOffFn)stopOsc, (ccFn)ccOsc);
}
/*!*****************************************************************************
*******************************************************************************
\note init_ros_servo
\date Dec. 1997
\remarks
initializes servo specific things
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
void
init_ros_servo(void)
{
int j, i;
char string[100];
if (ros_servo_initialized) {
printf("Task Servo is already initialized\n");
return;
}
// the servo name
sprintf(servo_name,"ros");
// initialize shared memories and shared semaphores
if (!init_shared_memory())
return;
/* inverse dynamics and other basic kinematic stuff*/
if (!init_dynamics())
return;
// initialize kinematics
init_kinematics();
// add variables to data collection
ros_servo_rate=servo_base_rate/task_servo_ratio;
initCollectData(ros_servo_rate);
for (i=1; i<=n_dofs; ++i) {
printf("%d...",i);
fflush(stdout);
sprintf(string,"%s_th",joint_names[i]);
addVarToCollect((char *)&(joint_state[i].th),string,(char *)"rad", DOUBLE,FALSE);
sprintf(string,"%s_thd",joint_names[i]);
addVarToCollect((char *)&(joint_state[i].thd),string,(char *)"rad/s", DOUBLE,FALSE);
sprintf(string,"%s_thdd",joint_names[i]);
addVarToCollect((char *)&(joint_state[i].thdd),string,(char *)"rad/s^2", DOUBLE,FALSE);
sprintf(string,"%s_u",joint_names[i]);
addVarToCollect((char *)&(joint_state[i].u),string,(char *)"Nm", DOUBLE,FALSE);
sprintf(string,"%s_ufb",joint_names[i]);
addVarToCollect((char *)&(joint_state[i].ufb),string,(char *)"Nm", DOUBLE,FALSE);
sprintf(string,"%s_load",joint_names[i]);
addVarToCollect((char *)&(joint_state[i].load),string,(char *)"Nm", DOUBLE,FALSE);
sprintf(string,"%s_des_th",joint_names[i]);
addVarToCollect((char *)&(joint_des_state[i].th),string,(char *)"rad",DOUBLE,FALSE);
sprintf(string,"%s_des_thd",joint_names[i]);
addVarToCollect((char *)&(joint_des_state[i].thd),string,(char *)"rad/s",DOUBLE,FALSE);
sprintf(string,"%s_des_thdd",joint_names[i]);
addVarToCollect((char *)&(joint_des_state[i].thdd),string,(char *)"rad/s^2",DOUBLE,FALSE);
sprintf(string,"%s_uff",joint_names[i]);
addVarToCollect((char *)&(joint_des_state[i].uff),string,(char *)"Nm",DOUBLE,FALSE);
}
// Cartesian variables
for (i=1; i<=n_endeffs; ++i) {
sprintf(string,"%s_x",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].x[_X_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_y",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].x[_Y_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_z",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].x[_Z_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_xd",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].xd[_X_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_yd",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].xd[_Y_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_zd",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].xd[_Z_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_xdd",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].xdd[_X_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_ydd",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].xdd[_Y_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_zdd",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].xdd[_Z_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_ad",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].ad[_X_]),string,(char *)"rad/s",DOUBLE,FALSE);
sprintf(string,"%s_bd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].ad[_Y_]),string,(char *)"rad/s",DOUBLE,FALSE);
sprintf(string,"%s_gd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].ad[_Z_]),string,(char *)"rad/s",DOUBLE,FALSE);
sprintf(string,"%s_add",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].add[_X_]),string,(char *)"rad/s2",DOUBLE,FALSE);
sprintf(string,"%s_bdd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].add[_Y_]),string,(char *)"rad/s2",DOUBLE,FALSE);
sprintf(string,"%s_gdd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].add[_Z_]),string,(char *)"rad/s2",DOUBLE,FALSE);
sprintf(string,"%s_des_x",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].x[_X_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_y",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].x[_Y_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_z",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].x[_Z_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_xd",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].xd[_X_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_yd",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].xd[_Y_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_zd",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].xd[_Z_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_xdd",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].xdd[_X_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_ydd",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].xdd[_Y_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_zdd",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].xdd[_Z_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_ad",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].ad[_X_]),string,(char *)"rad/s",DOUBLE,FALSE);
sprintf(string,"%s_des_bd",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].ad[_Y_]),string,(char *)"rad/s",DOUBLE,FALSE);
sprintf(string,"%s_des_gd",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].ad[_Z_]),string,(char *)"rad/",DOUBLE,FALSE);
sprintf(string,"%s_q0",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].q[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q1",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].q[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q2",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].q[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q3",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].q[_Q3_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q0d",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qd[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q1d",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qd[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q2d",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qd[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q3d",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qd[_Q3_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q0dd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qdd[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q1dd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qdd[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q2dd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qdd[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q3dd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qdd[_Q3_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q0",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].q[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q1",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].q[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q2",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].q[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q3",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].q[_Q3_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q0d",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qd[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q1d",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qd[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q2d",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qd[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q3d",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qd[_Q3_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q0dd",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q1dd",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q2dd",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q3dd",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q3_]),string,(char *)"-",DOUBLE,FALSE);
}
// misc sensors
for (i=1; i<=n_misc_sensors; ++i) {
sprintf(string,"%s",misc_sensor_names[i]);
addVarToCollect((char *)&(misc_sensor[i]),string,(char *)"-",DOUBLE,FALSE);
}
// the state of the base
addVarToCollect((char *)&(base_state.x[_X_]),(char *)"base_x",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.x[_Y_]),(char *)"base_y",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.x[_Z_]),(char *)"base_z",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xd[_X_]),(char *)"base_xd",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xd[_Y_]),(char *)"base_yd",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xd[_Z_]),(char *)"base_zd",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xdd[_X_]),(char *)"base_xdd",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xdd[_Y_]),(char *)"base_ydd",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xdd[_Z_]),(char *)"base_zdd",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.q[_Q0_]),(char *)"base_q0",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.q[_Q1_]),(char *)"base_q1",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.q[_Q2_]),(char *)"base_q2",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.q[_Q3_]),(char *)"base_q3",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.qd[_Q0_]),(char *)"base_qd0",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.qd[_Q1_]),(char *)"base_qd1",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.qd[_Q2_]),(char *)"base_qd2",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.qd[_Q3_]),(char *)"base_qd3",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.ad[_A_]),(char *)"base_ad",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.ad[_B_]),(char *)"base_bd",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.ad[_G_]),(char *)"base_gd",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.add[_A_]),(char *)"base_add",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.add[_B_]),(char *)"base_bdd",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.add[_G_]),(char *)"base_gdd",(char *)"-",DOUBLE,FALSE);
printf("done\n");
updateDataCollectScript();
// add to man pages
addToMan((char *)"status",(char *)"displays information about the servo",status);
addToMan((char *)"drs",(char *)"disables the ros servo",drs);
int argc = 1;
char name[] = "SL2ROS_Publisher";
char* argv[1];
argv[0] = name;
ros::init(argc, argv, "SL2ROS_Publisher");
if (!read_parameter_pool_int(config_files[PARAMETERPOOL], "default_publisher_enabled", &default_publisher_enabled_))
default_publisher_enabled_ = 0;
if(default_publisher_enabled_ && !ros_communicator_.initialize())
{
printf("ERROR: could not initialize ros communication\n");
return;
}
// initializes user specific issues
init_user_ros();
// if all worked out, we mark the servo as ready to go
ros_servo_initialized = TRUE;
// set oscilloscope to start value
initOsc();
setOsc(d2a_cr,0.0);
scd();
return;
}
/*!*****************************************************************************
*******************************************************************************
\note init_simulation_servo
\date Nov. 2007
\remarks
initializes everything for this servo
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
init_simulation_servo(void)
{
int i, j;
// the servo name
sprintf(servo_name,"sim");
// initialization of variables
simulation_servo_rate = servo_base_rate;
// read controller gains
controller_gain_th = my_vector(1,n_dofs);
controller_gain_thd = my_vector(1,n_dofs);
controller_gain_int = my_vector(1,n_dofs);
if (!read_gains(config_files[GAINS],controller_gain_th,
controller_gain_thd, controller_gain_int))
return FALSE;
/* inverse dynamics */
if (!init_dynamics())
return FALSE;
/* initialize kinematicss */
init_kinematics();
// get shared memory
if (!init_shared_memory())
return FALSE;
// object handling
if (!initObjects())
return FALSE;
// need sensor offsets
if (!read_sensor_offsets(config_files[SENSOROFFSETS]))
return FALSE;
// initializes user specific issues
if (!init_user_simulation())
return FALSE;
// add to man pages
addToMan("setIntRate","set number of integration cycles",setIntRate);
addToMan("setIntMethod","set integration method",setIntMethod);
addToMan("realTime","toggle real-time processing",toggleRealTime);
addToMan("status","displays status information about servo",status);
addToMan("dss","disables the simulation servo",dss);
addToMan("where_gains","Print gains of the joints",where_gains);
// data collection
initDataCollection();
// oscilloscope
initOsc();
// initialize user specific simulations
initUserSim(); // general initialization
if (!initUserSimulation()) // user specific intialization
return FALSE;
servo_enabled = TRUE;
return TRUE;
}
