void balancing(){ //thread balancing function
while(1){
angle += imu.getRoll();
usleep(50);
angle += imu.getRoll();
usleep(50);
angle += imu.getRoll();
usleep(50);
angle += imu.getRoll();
usleep(50);
angle =angle/4*180/3.1415;
pid.calculate_speed(angle,silniki.getLeftSpeed(),silniki.getRightSpeed(),steering,throttle,finalleftspeed,finalrightspeed);
if(balancingactive)silniki.setSpeed(finalleftspeed,finalrightspeed,0.0,4);
//printf("%f\n",angle);
if(angle>40.0){
actual = Position::layfront;
balancingactive = 0;
}
else if (angle<-40.0){
actual = Position::layback;
balancingactive = 0;
}
else actual = Position::standing;
angle = 0.0;
}
}
int main(int argc, char **argv)
{
// Init ros and create a ros publisher
ros::init(argc, argv, "talker");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
ros::Rate loop_rate(10);
// Create and init serial listenner
Imu imu;
imu.init();
// Main loop
int count = 0;
while (ros::ok()) {
// std_msgs::String msg;
// msg.data = "test";
// ROS_INFO("%s", msg.data.c_str());
// chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
int main()
{
Imu imu;
imu.init();
while(1)
{
imu.update();
cout<<"\n"<<setprecision(3)<<double(imu.yaw);
}
return 0;
}
int main()
{
bootup_files();
Imu imu;
imu.init();
while(1)
{
imu.update();
cout<<"\nYAW :"<<imu.yaw;
cout<<"\nROLL :"<<imu.roll;
cout<<"\nPITCH :"<<imu.pitch;
cout<<"\n\n\n";
// usleep(1000000);
}
return 0;
}
void* walk_thread(void*)
{
Imu imu;
// printf("GENORAI");
imu.init();
foot foot1[1000];
Communication comm;
AcYut bot(&comm,&imu);
Walk walk(&bot);
int j=0,i=0;
PathPacket pathpackvarlocal;
// Call walkthread here instead of reading file.
pthread_mutex_lock(&mutex_pathpacket);
pathpackvarlocal = pathpackvar;
pthread_mutex_unlock(&mutex_pathpacket);
footstepmain(10 , foot1[j].delta_y , j%2 , foot1 , i , pathpackvarlocal);
while (1)
{
printf("in walk thread\n");
j = 0;
while (j<i-1 && j<10)
{
walk.dribble(foot1[j].delta_y/2,foot1[j].delta_x,foot1[j].delta_theta,0);
printf("theta = %f delta_x = %f \n" , foot1[j].delta_theta , foot1[j].delta_x);
j++;
}
j--;
pthread_mutex_lock(&mutex_pathpacket);
pathpackvarlocal = pathpackvar;
pthread_mutex_unlock(&mutex_pathpacket);
// printf("%f\n" ,pathpackvarlocal.no_of_points);
footstepmain(walk.velocity2() , foot1[j].delta_y , j%2 , foot1 , i , pathpackvarlocal);
}
//walk.turnright(35.;
//while(1)
// balanceStatic(bot,1,DSP);
return 0;
};
void* readerThreadFunction(void* data)
{
int count = 0;
Imu *imu = (Imu*) data;
while(1)
{
imu->__update();
count++;
if(count == 20)
{
count = 0;
imu->__flush();
}
usleep(10000);
if(!(imu->threadActive))
break;
}
printf("IMU thread closing\n");
pthread_exit(NULL);
}
int main(void)
{
#ifdef SWITCH_IS_ON
pthread_create(&thread_id_switch,NULL,switchupdate,NULL);
#endif
#ifdef WALK_IS_ON
pthread_create (&thread_id_walk, NULL, walk_thread, NULL);
#endif
#ifdef GC_IS_ON
pthread_create (&thread_id_gc, NULL, readGameController, NULL);
#endif
#ifdef IMU_IS_ON
imu.init();
fstream f1;
f1.open("Source/xsens/imuyaw.angle", ios::in);
f1>>IMU_INITIAL_ANGLE;
f1.close();
#endif
registerXABSL();
start();
#ifdef GC_IS_ON
pthread_join(thread_id_gc,NULL);
#endif
#ifdef WALK_IS_ON
pthread_join(thread_id_walk,NULL);
#endif
#ifdef SWITCH_IS_ON
pthread_join(thread_id_switch,NULL);
#endif
return 0;
}
void ConfigFile::processVehicle(const xmlpp::Node* node,Vehicle &vehicle){
xmlpp::Node::NodeList list = node->get_children();
for(xmlpp::Node::NodeList::iterator iter = list.begin(); iter != list.end(); ++iter){
xmlpp::Node* child=dynamic_cast<const xmlpp::Node*>(*iter);
if(child->get_name()=="name")
extractStringChar(child,vehicle.name);
else if(child->get_name()=="file"){
string aux;
extractStringChar(child,aux);
processURDFFile(aux,vehicle);
}
else if(child->get_name()=="position")
extractPositionOrColor(child,vehicle.position);
else if(child->get_name()=="orientation")
extractOrientation(child,vehicle.orientation);
else if (child->get_name()=="jointValues")
processJointValues(child,vehicle.jointValues,vehicle.ninitJoints);
else if (child->get_name()=="virtualCamera"){
Vcam aux;
aux.init();
processVcam(child,aux);
vehicle.Vcams.push_back(aux);
}
else if (child->get_name()=="virtualRangeImage"){
Vcam aux;
aux.init();
aux.range=1;
processVcam(child,aux);
vehicle.VRangecams.push_back(aux);
} else if (child->get_name()=="rangeSensor"){
rangeSensor aux;
aux.init();
processRangeSensor(child,aux);
vehicle.range_sensors.push_back(aux);
} else if (child->get_name()=="objectPicker"){
rangeSensor aux;
aux.init();
processRangeSensor(child,aux);
vehicle.object_pickers.push_back(aux);
} else if (child->get_name()=="imu"){
Imu aux;
aux.init();
processImu(child,aux);
vehicle.imus.push_back(aux);
} else if (child->get_name()=="pressureSensor"){
XMLPressureSensor aux;
aux.init();
processPressureSensor(child,aux);
vehicle.pressure_sensors.push_back(aux);
} else if (child->get_name()=="gpsSensor"){
XMLGPSSensor aux;
aux.init();
processGPSSensor(child,aux);
vehicle.gps_sensors.push_back(aux);
} else if (child->get_name()=="dvlSensor"){
XMLDVLSensor aux;
aux.init();
processDVLSensor(child,aux);
vehicle.dvl_sensors.push_back(aux);
} else if (child->get_name()=="multibeamSensor"){
XMLMultibeamSensor aux;
aux.init();
processMultibeamSensor(child,aux);
vehicle.multibeam_sensors.push_back(aux);
}
}
}
int main(void)
{
silniki.disable();
imu.setup(); // initialize IMU
usleep(1000);
silniki.enable();
//if(!lipol.isGood())printf("niski poziom napiecia baterii");
imu.resetFIFO();
pid.timerStart(); //start pid timer
std::thread balance(balancing); //create balancing thread
usleep(500000);
usleep(500000);
while(1){
switch (actual){
case standing:
balancingactive =1;
usleep(200000);
break;
case layfront:
std::cout<<"I'm trying to stand front"<<std::endl;
silniki.setSpeed(0.0,0.0,0.0,4);
silniki.disable();
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
silniki.enable();
silniki.setSpeed(400.0,400.0,0.0,1);
usleep(200000);
silniki.setSpeed(500.0,500.0,0.0,1);
usleep(200000);
silniki.setSpeed(550.0,550.0,0.0,1);
usleep(500000);
silniki.setSpeed(-400.0,-400.0,0.0,1);
usleep(150000);
silniki.setSpeed(-500.0,-500.0,0.0,1);
usleep(50000);
actual = Position::standing;
std::cout<<"I'm standing"<<std::endl;
break;
case layback:
std::cout<<"I'm trying to stand back"<<std::endl;
silniki.setSpeed(0.0,0.0,0.0,1);
silniki.disable();
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
silniki.enable();
silniki.setSpeed(-400.0,-400.0,0.0,1);
usleep(200000);
silniki.setSpeed(-500.0,-500.0,0.0,1);
usleep(200000);
silniki.setSpeed(-550.0,-550.0,0.0,1);
usleep(500000);
silniki.setSpeed(350.0,350.0,0.0,1);
usleep(100000);
silniki.setSpeed(550.0,550.0,0.0,1);
usleep(50000);
actual = Position::standing;
std::cout<<"I'm standing"<<std::endl;
break;
}
}
balance.detach();
return 0;
}
int main()
{
/* left leg up(390 - x) and out(+ve z). right leg inward. right arm outward. change in order of tens
*/
Imu imu;
imu.init();
Communication comm;
AcYut bot(&comm,NULL);
// LEFT HAND
float mot07[]={4096-2172,4096-2500,4096-2172,4096-2500,4096-2172,4096-2500,4096-2172};//float mot07[]={4096-2048,4096-2048,4096-2048,4096-2048,4096-2048,4096-2048,4096-2048};
float mot08[]={4096-1040,4096-1040,4096-1040,4096-1040,4096-1040,4096-1040,4096-1040};//float mot08[]={4096-1040,4096-1040,4096-1040,4096-1040,4096-1040,4096-1040,4096-1040};
float mot10[]={4096-2198,4096-2700,4096-3198,4096-2700,4096-3198,4096-2700,4096-3198};//float mot10[]={4096-2048,4096-2048,4096-2048,4096-2048,4096-2048,4096-2048,4096-2048};
//RIGHT HAND
float mot27[]={2172,2500,2172,2500,2172,2500,2172}; //float mot27[]={2048,3072,3072,3072,3072,2048,2048};
float mot28[]={1040,1140,1040,1040,1040,1040,1040}; //float mot28[]={1100,2048,2048,2048,2048,1100,1100};
float mot30[]={2048,2550,3048,2550,3048,2550,3048}; //float mot30[]={2048,2048,3072,2048,3072,2048,2048};
//LEFT LEG
float llegx[]={390,370,360,350,340,340,340};
float llegy[]={0,0,0,0,0,0,0};
float llegz[]={0,10,20,20,20,20,20};
//RIGHT LEG
float rlegx[]={390,390,390,390,390,390,390};
float rlegy[]={0,0,0,0,0,0,0};
float rlegz[]={0,-50,-60,-60,-60,-60,-60};
//TIME
float t1[]={0,1.5,2,3.5};
float t=6.0;
int fps=60;
int vm7,vm8,vm10,vm27,vm28,vm30;
float vlx,vly,vlz,vrx,vry,vrz;
int i;
int t3;
int arr_l[4],arr_r[4];
for(t3=0;t3<360;t3+=1)
{
int j= (int)(t3/60.0);
arr_l[0]=vm7=(int)scurve(mot07[j],mot07[j+1],t3%60, 60);
arr_l[1]=vm8=(int)scurve(mot08[j],mot08[j+1],t3%60, 60);
arr_l[3]=vm10=(int)scurve(mot10[j],mot10[j+1],t3%60, 60);
arr_r[0]=vm27=(int)scurve(mot27[j],mot27[j+1],t3%60, 60);
arr_r[1]=vm28=(int)scurve(mot28[j],mot28[j+1],t3%60, 60);
arr_r[3]=vm30=(int)scurve(mot30[j],mot30[j+1],t3%60, 60);
vlx=scurve(llegx[j],llegx[j+1],t3%60, 60);
vly=scurve(llegy[j],llegy[j+1],t3%60, 60);
vlz=scurve(llegz[j],llegz[j+1],t3%60, 60);
vrx=scurve(rlegx[j],rlegx[j+1],t3%60, 60);
vry=scurve(rlegy[j],rlegy[j+1],t3%60, 60);
vrz=scurve(rlegz[j],rlegz[j+1],t3%60, 60);
arr_l[2]=arr_r[2]=0;
bot.left_hand->setGoalPositionSync(arr_l);
bot.right_hand->setGoalPositionSync(arr_r);
bot.left_leg->runIK(vlx,vly,vlz,0);
bot.left_leg->setGoalPositionSync();
bot.right_leg->runIK(vrx,vry,vrz,0);
bot.right_leg->setGoalPositionSync();
comm.syncFlush();
usleep(16666);
printf("%d %d %d %d %d %d %f %f %f %f %f %f\n",vm7,vm8,vm10,vm27,vm28,vm30,vlx,vly,vlz,vrx,vry,vrz);
}
return 0;
}
void wave()
{
Imu imu;
imu.init();
Communication comm;
AcYut bot(&comm,NULL);
// LEFT HAND
float mot07[]={4096-2048,4096-2048,4096-2048,4096-2048,4096-2048,4096-2048,4096-2048};
float mot08[]={4096-1100,4096-1100,4096-1100,4096-1100,4096-1100,4096-1100,4096-1100};
float mot10[]={4096-2198,4096-2198,4096-2198,4096-2198,4096-2198,4096-2198,4096-2198};
//RIGHT HAND
float mot27[]={2048,3072,3072,3072,3072,2048,2048};
float mot28[]={1100,2048,2048,2048,2048,1100,1100};
float mot30[]={2048,2048,3072,2048,3072,2048,2048};
//LEFT LEG
float llegx[]={390,390,390,390,390,390,390};
float llegy[]={0,0,0,0,0,0,0};
float llegz[]={0,0,0,0,0,0,0};
//RIGHT LEG
float rlegx[]={390,390,390,390,390,390,390};
float rlegy[]={0,0,0,0,0,0,0};
float rlegz[]={0,0,0,0,0,0,0};
//TIME
float t1[]={0,1.5,2,3.5};
float t=6.0;
int fps=60;
int vm7,vm8,vm10,vm27,vm28,vm30;
float vlx,vly,vlz,vrx,vry,vrz;
int i;
int t3;
int arr_l[4],arr_r[4];
for(t3=0;t3<360;t3+=1)
{
int j= (int)(t3/60.0);
arr_l[0]=vm7=(int)scurve(mot07[j],mot07[j+1],t3%60, 60);
arr_l[1]=vm8=(int)scurve(mot08[j],mot08[j+1],t3%60, 60);
arr_l[3]=vm10=(int)scurve(mot10[j],mot10[j+1],t3%60, 60);
arr_r[0]=vm27=(int)scurve(mot27[j],mot27[j+1],t3%60, 60);
arr_r[1]=vm28=(int)scurve(mot28[j],mot28[j+1],t3%60, 60);
arr_r[3]=vm30=(int)scurve(mot30[j],mot30[j+1],t3%60, 60);
vlx=scurve(llegx[j],llegx[j+1],t3%60, 60);
vly=scurve(llegy[j],llegy[j+1],t3%60, 60);
vlz=scurve(llegz[j],llegz[j+1],t3%60, 60);
vrx=scurve(rlegx[j],rlegx[j+1],t3%60, 60);
vry=scurve(rlegy[j],rlegy[j+1],t3%60, 60);
vrz=scurve(rlegz[j],rlegz[j+1],t3%60, 60);
arr_l[2]=arr_r[2]=0;
bot.left_hand->setGoalPositionSync(arr_l);
bot.right_hand->setGoalPositionSync(arr_r);
bot.left_leg->runIK(vlx,vly,vlz,0);
bot.left_leg->setGoalPositionSync();
bot.right_leg->runIK(vrx,vry,vrz,0);
bot.right_leg->setGoalPositionSync();
comm.syncFlush();
usleep(16666);
printf("%d %d %d %d %d %d %f %f %f %f %f %f\n",vm7,vm8,vm10,vm27,vm28,vm30,vlx,vly,vlz,vrx,vry,vrz);
}
printf("Move completed\n");
}
int main(int argc, char** argv){
std::string path = "/dev/ttyACM0";
Imu imu = Imu(path, 42, .007);
State new_data = {0.0};
//int cal = imu.calibrate();
//logging
/*std::string log_filename = "file.txt";
logger logger(log_filename, 100, true);
Data_log d;*/
ros::init(argc,argv,"imu");
ros::NodeHandle n;
ros::Publisher imu_pub;
imu_pub = n.advertise<imu::ImuData>("imu/imu_data",1);
//int cal = imu.calibrate();
float psi_t = 0;
int suc = 0;
int cal = imu.calibrate();
std::queue<double> phi_ave, theta_ave, phi_dot_ave, theta_dot_ave, psi_dot_ave;
while(ros::ok())
{
//begin = ros::Time::now().toSec();
////int suc = imu.get_imu_calibrated_data(imu_data);
suc = imu.get_imu_calibrated_data(new_data);
/*psi_t = psi_t + new_data.dt * new_data.psi_dot_cal*.0065;
printf("test psi = %f \n",psi_t);*/
if(suc == 1)
{
//printf("imu_data.psi = %f \n", imu_data.psi);
imu::ImuData imuMsg;
imuMsg.phi = new_data.phi;
imuMsg.theta = new_data.theta;
imuMsg.psi_magn_continuous = new_data.psi_magn_raw;
imuMsg.psi_magn_continuous_calibrated = new_data.psi_magn_continuous_calibrated;
imuMsg.psi = new_data.psi_gyro_integration;
imuMsg.phi_dot = new_data.phi_dot_cal;
imuMsg.theta_dot = new_data.theta_dot_cal;
imuMsg.psi_dot = new_data.psi_dot_cal;
imuMsg.psi_gyro_integration = new_data.psi_gyro_integration;
imuMsg.altitude_calibrated = new_data.altitude_calibrated;
/*imuMsg.theta_uncal = new_data.theta_uncal;
imuMsg.phi_uncal = new_data.phi_uncal;
imuMsg.theta_dot_uncal = new_data.theta_dot_uncal;
imuMsg.phi_dot_uncal = new_data.phi_dot_uncal;
*/
/*
if(phi_ave.size() < 1000)
{
phi_ave.push(phi_uncal);
}
else
{
phi_ave.pop();
phi_ave.push(phi_uncal);
}
for(int c=0;c<
*/
imuMsg.dt = new_data.dt;
imuMsg.succ_read = new_data.succ_read;
imu_pub.publish(imuMsg);
//end = ros::Time::now().toSec();
//printf("%f \n" , end-begin);
} else if (suc == 2) {
//printf("suc = %i \n",suc);
//imu::ImuData imuMsg;
//imuMsg.succ_read = new_data.succ_read;
//imu_pub.publish(imuMsg);
}
}
return 1;
}
/*
int balanceStatic(AcYut& bot, int phase, int leg = DSP)
{
sleep(1);
const double (&COM)[AXES] = bot.getRotCOM();
//printf("COM X\t%3.3lf\tY\t%3.3lf\tZ\t%3.3lf\n",COM[0],COM[1],COM[2]);
printf("R\t%3.3lf\tP\t%3.3lf\n",bot.imu->roll,bot.imu->pitch);
const supportPolygon poly = bot.calcSupportPolygon();
const Point center = getPolyCentroid(bot.calcSupportPolygon());
printf("\n\n");
float error[AXES] = {0};
float corr[AXES] = {0};
error[Y] = COM[Y] - center.y;
error[Z] = COM[Z] - center.x;
corr[Y] = 0.2 * error[Y];
corr[Z] = 0.2 * error[Z];
const float (&leftLeg)[AXES+1] = bot.left_leg->getLegCoods();
const float (&rightLeg)[AXES+1] = bot.right_leg->getLegCoods();
printf("Left Leg\t%lf\t%lf\t%lf\n",leftLeg[X],leftLeg[Y],leftLeg[Z],leftLeg[4]);
printf("Right Leg\t%lf\t%lf\t%lf\n",rightLeg[X],rightLeg[Y],rightLeg[Z],rightLeg[4]);
bot.right_leg->runIK(rightLeg[X],rightLeg[Y] + corr[Y],rightLeg[Z],rightLeg[4]);
bot.updateBot();
printf("COMY\t%3.3lf\tZ\t%3.3lf\n",COM[1],COM[2]);
printf("CenY\t%3.3lf\tX\t%3.3lf\n",center.y,center.x);
printf("CorY\t%3.3lf\tZ\t%3.3lf\n",corr[Y],corr[Z]);
for(int i=0;i<100;i++);
printf("\n");
return 1;
}
int kick(AcYut *bot)
{
float kickTime = 1.0;
int fps = 80;
float frameTime = (float)1.0/(float)fps;
int sleep = frameTime * 1000000;
printf("FrameTime\t%lf\n",frameTime);
float kickHeight = 40;
float botHeight = 390;
float wtShift = 50;
float wtShiftTime= 0.5;
float liftTime = 0.5;
float counterBalance = 15;
float yStart=0, yrStart=0, zStart=0, zrStart=0;
float x,xr,y,yr,z,zr;
for(float time=0;time<wtShiftTime;time+=frameTime)
{
x = botHeight;
xr= botHeight;
y = yStart;
yr= yrStart;
z = scurve2(zStart, wtShift, time, wtShiftTime);
zr= scurve2(zrStart, -wtShift, time, wtShiftTime);
printf("Time\t%lf\tX\t%lf\tY\t%lf\tZ\t%lf\tXR\t%lf\tYR\t%lf\tZR\t%lf\n",time,x,y,z,xr,yr,zr);
bot->right_leg->runIK(x,y,z,0);
bot->left_leg->runIK(xr,yr,zr,0);
bot->updateBot();
usleep(sleep);
}
for(float time=0;time<liftTime;time+=frameTime)
{
x = scurve2(botHeight, botHeight - kickHeight, time, liftTime);
xr= botHeight;
y = yStart;
yr= yrStart;
z = scurve2(wtShift,wtShift+counterBalance,time,liftTime);
zr= scurve2(-wtShift,-wtShift - counterBalance, time, liftTime);
printf("Time\t%lf\tX\t%lf\tY\t%lf\tZ\t%lf\tXR\t%lf\tYR\t%lf\tZR\t%lf\n",time,x,y,z,xr,yr,zr);
bot->right_leg->runIK(x,y,z,0);
bot->left_leg->runIK(xr,yr,zr,0);
bot->updateBot();
usleep(sleep);
}
};
void test(AcYut *bot)
{
int xmax = 390;
int xmin = 360;
int ymax = 30;
int ymin = -30;
float time = 0.5;
float frameTime = 1.0/60;
float t = 0;
float x=xmax, y= ymin;
for(t=0;t<time;t+=frameTime)
{
x = linear(xmax,xmin,t,time);
y = linear(ymin,ymax,t,time);
bot->right_leg->runIK(x,y,0,0);
bot->updateBot();
usleep(16670);
printf("test x %lf y %lf\n",x,y);
getchar();
}
for(t=0;t<time;t+=frameTime)
{
x = linear(xmin,xmax,t,time);
y = linear(ymax,ymin,t,time);
bot->right_leg->runIK(x,y,0,0);
bot->updateBot();
usleep(16670);
printf("test x %lf y %lf\n",x,y);
getchar();
}
}
*/
int main()
{
Imu imu;
imu.init();
// (void) signal(SIGINT,doquit);
Communication comm;
AcYut bot(&comm,&imu);
/* bot.left_leg->runIK(390,0,0,0);
bot.right_leg->runIK(390,0,0,0);
for(int i=0;i<100;i++)
{
bot.left_leg->runIK(linear(390,100,i,100),0,0,0);
bot.right_leg->runIK(linear(390,100,i,100),0,0,0);
bot.updateBot();
usleep(50000);
printf("%d\n",i);
}
for(int i=0;i<100;i++)
{
bot.left_leg->runIK(linear(100,390,i,100),0,0,0);
bot.right_leg->runIK(linear(100,390,i,100),0,0,0);
bot.updateBot();
usleep(50000);
printf("%d\n",i);
}
*/
Walk walk(&bot);
// walk.turnright(90);
walk.dribble();
// walk.pathdribble(250, 0, 0, 0);
// walk.dribble();
// cout<<"Moving right"<<endl;
// walk.sideMotion(200);
// cout<<"Moving left"<<endl;
// walk.sideMotion(-200);
while(walk.velocity()*1.5<160)
{
walk.accelerate();
walk.dribble();
// printf("%f\n",walk.velocity());
}
// walk.stopMotion();
int i = 150;
int j = 0;
while(1)
{
j++;
// i++;
// if (!(i%5)){
// walk.setStrafe(-8);
// }
// walk.pathdribble(-1,20,0,0);//Strafing
// if (i<280)
// i+=15;
// cout<<i<<" "<<endl;
// walk.pathdribble(i,0,0,0*pow(-1,j%2));
walk.dribble();
// printf("%f\n",walk.velocity());
}
return 0;
};
//--------------------------------------------------------------
void ofApp::setup(){
gui = new ofxUICanvas();
calTime = 10000;
firstData = 0;
dataReceived = false;
debugdraw = true;
offset = ofVec3f(0,0,0);
state = 0; // 0 waiting for data - 1 calibrating - 2 receiving - 3 error
serial.listDevices();
vector<ofSerialDeviceInfo> devices = serial.getDeviceList();
int serialAdresses [] = {0
,1
// ,2
// ,3
// ,4
};
for(int i=0; i<NUM_SENSORS; i++) {
Imu * imu = new Imu();
imu->setup(serialAdresses[i]);
imus.push_back(imu);
gui->addLabel(devices.at(serialAdresses[i]).getDeviceName());
gui->addButton("Calibrate", false, 20, 20);
gui->addSpacer();
}
oscSender.setup("localhost", 14040);
//serial.setup(0, 57600);
//serial.flush();
animIsPaused = false;
animCurrentFrame = 0;
ofDisableSmoothing();
ofDisableAlphaBlending();
//cam.setupPerspective(false, 60, 0.1, 3000);
//mCam1.setPosition(500, 800, 800);
cam.lookAt(ofVec3f(0));
// Pelvis is root Maybe we should have another point on torso?
mSkeleton["Pelvis"] = JointP_t(new ofxJoint());
mSkeleton["Pelvis"]->setGlobalPosition(ofVec3f(0));
mSkeleton["Chest"] = JointP_t(new ofxJoint());
mSkeleton["Chest"]->setParent(mSkeleton["Pelvis"]);
mSkeleton["Chest"]->setPosition(ofVec3f(0, 100 , 0 ));
mSkeleton["Head"] = JointP_t(new ofxJoint());
mSkeleton["Head"]->setParent(mSkeleton["Chest"]);
mSkeleton["Head"]->setPosition(ofVec3f(0, 60 , 0 ));
string names[4] = {"Hip", "Knee", "Foot", "Toe"};
for (int i = 0; i < 4; i++){
mSkeleton["L_" + names[i]] = JointP_t(new ofxJoint());
mSkeleton["R_" + names[i]] = JointP_t(new ofxJoint());
}
// apply a limb's hierarchy
mSkeleton["L_Toe"]->bone(mSkeleton["L_Foot"])->bone(mSkeleton["L_Knee"])->bone(mSkeleton["L_Hip"])->bone(mSkeleton["Pelvis"]);
mSkeleton["R_Toe"]->bone(mSkeleton["R_Foot"])->bone(mSkeleton["R_Knee"])->bone(mSkeleton["R_Hip"])->bone(mSkeleton["Pelvis"]);
// set the limb's joints positions
mSkeleton["L_Hip"]->setGlobalPosition(ofVec3f(-40, -80 , 0 ));
mSkeleton["L_Hip"]->setOrientation(ofQuaternion(20 - 40, ofVec3f(0,1,0)));
mSkeleton["L_Knee"]->setPosition(ofVec3f(0, -100, 0));
mSkeleton["L_Foot"]->setPosition(ofVec3f(0, -80, 0));
mSkeleton["L_Toe"]->setPosition(ofVec3f(0, -5, 20));
mSkeleton["R_Hip"]->setGlobalPosition(ofVec3f(40, -80 , 0 ));
mSkeleton["R_Hip"]->setOrientation(ofQuaternion(20 - 40, ofVec3f(0,-1,0)));
mSkeleton["R_Knee"]->setPosition(ofVec3f(0, -100, 0));
mSkeleton["R_Foot"]->setPosition(ofVec3f(0, -80, 0));
mSkeleton["R_Toe"]->setPosition(ofVec3f(0, -5, 20));
string namesUpper[4] = {"Shoulder", "Elbow", "Hand", "Finger"};
for (int i = 0; i < 4; i++){
mSkeleton["L_" + namesUpper[i]] = JointP_t(new ofxJoint());
mSkeleton["R_" + namesUpper[i]] = JointP_t(new ofxJoint());
}
// apply a limb's hierarchy
mSkeleton["L_Finger"]->bone(mSkeleton["L_Hand"])->bone(mSkeleton["L_Elbow"])->bone(mSkeleton["L_Shoulder"])->bone(mSkeleton["Chest"]);
mSkeleton["R_Finger"]->bone(mSkeleton["R_Hand"])->bone(mSkeleton["R_Elbow"])->bone(mSkeleton["R_Shoulder"])->bone(mSkeleton["Chest"]);
// set the limb's joints positions
mSkeleton["L_Shoulder"]->setGlobalPosition(ofVec3f(-60, 100, 0 ));
mSkeleton["L_Shoulder"]->setOrientation(ofQuaternion(20 - 40, ofVec3f(0,1,0)));
mSkeleton["L_Elbow"]->setPosition(ofVec3f(-20, -80, 0));
mSkeleton["L_Hand"]->setPosition(ofVec3f(0, -80, 0));
mSkeleton["L_Finger"]->setPosition(ofVec3f(0, -20, 0));
mSkeleton["R_Shoulder"]->setGlobalPosition(ofVec3f(60, 100, 0 ));
mSkeleton["R_Shoulder"]->setOrientation(ofQuaternion(20 - 40, ofVec3f(0,-1,0)));
mSkeleton["R_Elbow"]->setPosition(ofVec3f(20, -80, 0));
mSkeleton["R_Hand"]->setPosition(ofVec3f(0, -80, 0));
mSkeleton["R_Finger"]->setPosition(ofVec3f(0, -20, 0));
// set joint names according to their map indices.
for (map<string, JointP_t>::iterator it = mSkeleton.begin(); it != mSkeleton.end(); ++it){
it->second->setName(it->first);
}
position = ofVec3f(0,0,0);
velocity = ofVec3f(0,0,0);
}
int main()
{
Imu imu;
imu.init();
Communication comm;
AcYut bot(&comm,NULL);
// LEFT HAND
float mot07[]={4096-2048,4096-2560,4096-1536,4096-2560,4096-1536,4096-2560,4096-1536,4096-2560,4096-2048,4096-2048};
float mot08[]={4096-1100,4096-2048,4096-2048,4096-2048,4096-2048,4096-2048,4096-2048,4096-2048,4096-1100,4096-1100};
float mot10[]={2048 - 150,4096-3000,4096-1600,4096-3000,4096-1600,4096-3000,4096-1600,4096-3000,2048 -150,2048 - 150};
//RIGHT HAND0
float mot27[]={2048,1536,2560,1536,2560,1536,2560,1536,2048,2048};
float mot28[]={1100,2048,2048,2048,2048,2048,2048,2048,1100,1100};
float mot30[]={2048,1600,3000,1600,3000,1600,3000,1600,2048,2048};
//LEFT LEG
float llegx[]={390,390,390,390,390,390,390,390,390,390};
float llegy[]={0,0,0,0,0,0,0,0,0,0};
float llegz[]={0,0,0,0,0,0,0,0,0,0};
//RIGHT LEG
float rlegx[]={390,390,390,390,390,390,390,390,390,390};
float rlegy[]={0,0,0,0,0,0,0,0,0,0};
float rlegz[]={0,0,0,0,0,0,0,0,0,0};
//TIME
float t1[]={0,1,2,3,4,5,6};
float t=6.0;
int fps=60;
int vm7,vm8,vm10,vm27,vm28,vm30;
float vlx,vly,vlz,vrx,vry,vrz;
int i;
int t3;
int arr_l[4],arr_r[4];
for(t3=0;t3<540;t3+=1)
{
int j= (int)(t3/60.0);
arr_l[0]=vm7=(int)scurve(mot07[j],mot07[j+1],t3%60, 60);
arr_l[1]=vm8=(int)scurve(mot08[j],mot08[j+1],t3%60, 60);
arr_l[3]=vm10=(int)scurve(mot10[j],mot10[j+1],t3%60, 60);
arr_r[0]=vm27=(int)scurve(mot27[j],mot27[j+1],t3%60, 60);
arr_r[1]=vm28=(int)scurve(mot28[j],mot28[j+1],t3%60, 60);
arr_r[3]=vm30=(int)scurve(mot30[j],mot30[j+1],t3%60, 60);
vlx=scurve(llegx[j],llegx[j+1],t3%60, 60);
vly=scurve(llegy[j],llegy[j+1],t3%60, 60);
vlz=scurve(llegz[j],llegz[j+1],t3%60, 60);
vrx=scurve(rlegx[j],rlegx[j+1],t3%60, 60);
vry=scurve(rlegy[j],rlegy[j+1],t3%60, 60);
vrz=scurve(rlegz[j],rlegz[j+1],t3%60, 60);
arr_l[2]=arr_r[2]=0;
bot.left_hand->setGoalPositionSync(arr_l);
bot.right_hand->setGoalPositionSync(arr_r);
bot.left_leg->runIK(vlx,vly,vlz,0);
bot.left_leg->setGoalPositionSync();
bot.right_leg->runIK(vrx,vry,vrz,0);
bot.right_leg->setGoalPositionSync();
comm.syncFlush();
// if((t3>=6*60)&&((t3%60)==0))
// usleep(33333);
usleep(16666);
printf("%d %d %d %d %d %d %f %f %f %f %f %f\n",vm7,vm8,vm10,vm27,vm28,vm30,vlx,vly,vlz,vrx,vry,vrz);
}
return 0;
}
