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()
{
/* 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;
}
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;
}
