void updateIncomingMessage(void) {
// Check for Message(s) And Handle If Necessary
String response_message = "";
while (communication.available()) { // read in message(s) until nothing in serial buffer
response_message += handleIncomingMessage();
}
// Append Responses From Message(s) Then Send
if (response_message != "") {
response_message = "\"GTYP\":\"Response\"," + response_message;
response_message += "\"GEND\":0";
communication.send(response_message);
}
}
void *
Communication::ReadThread (void *p)
{
Communication *comm = (Communication *)p;
Log *log = lldb_private::GetLogIfAllCategoriesSet (LIBLLDB_LOG_COMMUNICATION);
if (log)
log->Printf ("%p Communication::ReadThread () thread starting...", p);
uint8_t buf[1024];
Error error;
ConnectionStatus status = eConnectionStatusSuccess;
bool done = false;
while (!done && comm->m_read_thread_enabled)
{
status = comm->BytesAvailable (UINT32_MAX, &error);
if (status == eConnectionStatusSuccess)
{
size_t bytes_read = comm->ReadFromConnection (buf, sizeof(buf), status, &error);
if (bytes_read > 0)
comm->AppendBytesToCache (buf, bytes_read, true);
}
switch (status)
{
case eConnectionStatusSuccess:
break;
case eConnectionStatusNoConnection: // No connection
case eConnectionStatusLostConnection: // Lost connection while connected to a valid connection
done = true;
// Fall through...
default:
case eConnectionStatusError: // Check GetError() for details
case eConnectionStatusTimedOut: // Request timed out
error.LogIfError(log, "%p Communication::BytesAvailable () => status = %i", p, status);
break;
}
}
if (log)
log->Printf ("%p Communication::ReadThread () thread exiting...", p);
// Let clients know that this thread is exiting
comm->m_read_thread = LLDB_INVALID_HOST_THREAD;
comm->BroadcastEvent (eBroadcastBitReadThreadDidExit);
return NULL;
}
void *
Communication::ReadThread (void *p)
{
Communication *comm = (Communication *)p;
Log *log(lldb_private::GetLogIfAllCategoriesSet (LIBLLDB_LOG_COMMUNICATION));
if (log)
log->Printf ("%p Communication::ReadThread () thread starting...", p);
uint8_t buf[1024];
Error error;
ConnectionStatus status = eConnectionStatusSuccess;
bool done = false;
while (!done && comm->m_read_thread_enabled)
{
size_t bytes_read = comm->ReadFromConnection (buf, sizeof(buf), 5 * TimeValue::MicroSecPerSec, status, &error);
if (bytes_read > 0)
comm->AppendBytesToCache (buf, bytes_read, true, status);
else if ((bytes_read == 0)
&& status == eConnectionStatusEndOfFile)
{
if (comm->GetCloseOnEOF ())
comm->Disconnect ();
comm->AppendBytesToCache (buf, bytes_read, true, status);
}
switch (status)
{
case eConnectionStatusSuccess:
break;
case eConnectionStatusEndOfFile:
if (comm->GetCloseOnEOF())
done = true;
break;
case eConnectionStatusNoConnection: // No connection
case eConnectionStatusLostConnection: // Lost connection while connected to a valid connection
done = true;
// Fall through...
case eConnectionStatusError: // Check GetError() for details
case eConnectionStatusTimedOut: // Request timed out
if (log)
error.LogIfError (log,
"%p Communication::ReadFromConnection () => status = %s",
p,
Communication::ConnectionStatusAsCString (status));
break;
}
}
log = lldb_private::GetLogIfAllCategoriesSet (LIBLLDB_LOG_COMMUNICATION);
if (log)
log->Printf ("%p Communication::ReadThread () thread exiting...", p);
// Let clients know that this thread is exiting
comm->BroadcastEvent (eBroadcastBitReadThreadDidExit);
return NULL;
}
String handleIncomingMessage(void) {
// Parse Message into: Instruction Code - ID - Parameter
String return_message = "";
String incoming_message = communication.receive();
Instruction instruction = parseIncomingMessage(incoming_message);
// Pass Parsed Message To All Objects and Update Return Message if Applicable
if (instruction.valid) {
//return_message += sensor_dfr01610300_water_ph_temperature_ec_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += sensor_venier_ph_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += sensor_vernier_ec_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += sensor_ds18b20_water_temperature.set(instruction.code, instruction.id, instruction.parameter);
return_message += sensor_tsl2561_light_intensity_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += sensor_dht22_air_temperature_humidity_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += sensor_gc0011_air_co2_temperature_humidity_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += sensor_contact_switch_general_shell_open_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += sensor_contact_switch_general_window_open_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += actuator_relay_air_heater_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += actuator_relay_air_humidifier_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += actuator_relay_air_vent_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += actuator_relay_air_circulation_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += actuator_relay_light_panel_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += actuator_relay_light_chamber_illumination_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += actuator_relay_light_motherboard_illumination_default.set(instruction.code, instruction.id, instruction.parameter);
}
return return_message;
}
PolizElem* PolizBuild::EvaluateFun(PolizItem **stack) const {
char cmd[8] = "build\n";
printf("cmd_build\n");
server.send_message(cmd);
robot.begin_constructing();
return 0;
}
void updateStreamMessage(void) {
// Initialize Stream Message
String stream_message = "\"GTYP\":\"Stream\",";
// Get Stream Message
stream_message += sensor_water_ph_temperature_ec_default.get();
stream_message += sensor_tsl2561_light_intensity_default.get();
stream_message += sensor_dht22_air_temperature_humidity_default.get(); // does not work on 1.0
stream_message += sensor_gc0011_air_co2_temperature_humidity_default.get();
stream_message += actuator_relay_air_heater_default.get();
stream_message += actuator_relay_air_humidifier_default.get();
stream_message += actuator_relay_air_vent_default.get();
stream_message += actuator_relay_air_circulation_default.get();
stream_message += chamber_illumination_default.get();
stream_message += dose_pump1_default.get();
stream_message += dose_pump2_default.get();
stream_message += dose_pump3_default.get();
stream_message += dose_pump4_default.get();
stream_message += actuator_relay_airpump_default.get();
stream_message += actuator_relay_co2_default.get();
stream_message += actuator_relay_watercir_default.get();
stream_message += actuator_relay_coolvalve_default.get();
// Return Stream Message
stream_message += "\"GEND\":0";
// Send Stream Message
communication.send(stream_message);
}
String handleIncomingMessage(void) {
// Parse Message into: Instruction Code - ID - Parameter
String return_message = "";
String incoming_message = communication.receive();
Instruction instruction = parseIncomingMessage(incoming_message);
// Pass Parsed Message To All Objects and Update Return Message if Applicable
if (instruction.valid) {
return_message += sensor_water_ph_temperature_ec_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += sensor_tsl2561_light_intensity_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += sensor_dht22_air_temperature_humidity_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += sensor_gc0011_air_co2_temperature_humidity_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += actuator_relay_air_heater_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += actuator_relay_air_humidifier_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += actuator_relay_air_vent_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += actuator_relay_air_circulation_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += chamber_illumination_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += dose_pump1_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += dose_pump2_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += dose_pump3_default.set(instruction.code, instruction.id, instruction.parameter);
return_message += dose_pump4_default.set(instruction.code, instruction.id, instruction.parameter);
}
return return_message;
}
void updateStreamMessage(void) {
// Initialize Stream Message
String stream_message = "\"GTYP\":\"Stream\",";
// Get Stream Message
//stream_message += sensor_dfr01610300_water_ph_temperature_ec_default.get();
stream_message += sensor_venier_ph_default.get();
stream_message += sensor_vernier_ec_default.get();
stream_message += sensor_ds18b20_water_temperature.get();
stream_message += sensor_tsl2561_light_intensity_default.get();
stream_message += sensor_dht22_air_temperature_humidity_default.get(); // does not work on 1.0
stream_message += sensor_gc0011_air_co2_temperature_humidity_default.get();
stream_message += sensor_contact_switch_general_shell_open_default.get();
stream_message += sensor_contact_switch_general_window_open_default.get();
stream_message += actuator_relay_air_heater_default.get();
stream_message += actuator_relay_air_humidifier_default.get();
stream_message += actuator_relay_air_vent_default.get();
stream_message += actuator_relay_air_circulation_default.get();
stream_message += actuator_relay_light_panel_default.get();
stream_message += actuator_relay_light_chamber_illumination_default.get();
stream_message += actuator_relay_light_motherboard_illumination_default.get();
// Return Stream Message
stream_message += "\"GEND\":0";
// Send Stream Message
communication.send(stream_message);
}
int main(int argc,char *argv[]){
/* ----------------------------------------------------------- */
// 同期通信クラスのインスタンス化
/* ----------------------------------------------------------- */
Communication *serverdata = new Communication(argc,argv);
if(serverdata -> getMyrank() != 0){
puts("server is not rank 0.");
return 1;
}else
printf("I'm server. My rank is %d\n",serverdata->getMyrank());
/* ----------------------------------------------------------- */
/* ----------------------------------------------------------- */
// ノードからの受信
/* ----------------------------------------------------------- */
//before(iNodenum) -> after(serverdata->getNodenum())
printf("node num %d\n",serverdata->getNodenum());
for(int i =0;i<serverdata->getNodenum()-1;i++){
printf("received nodeRank %d\n",serverdata->procrankReceive());
}
/* ----------------------------------------------------------- */
/* ----------------------------------------------------------- */
// MPI_Barrier();
// -> 全プロセスがこの関数を呼ぶまでブロック
/* ----------------------------------------------------------- */
MPI_Barrier(MPI_COMM_WORLD);
/* ----------------------------------------------------------- */
printf("end MPI_Barrier\n");
/* ----------------------------------------------------------- */
// デストラクト
/* ----------------------------------------------------------- */
delete (serverdata);
serverdata = NULL;
/* ----------------------------------------------------------- */
return EXIT_SUCCESS;
}
PolizElem* PolizEndTurn::EvaluateFun(PolizItem **stack) const {
char cmd0[32] = "# Trading results";
char cmd[16] = "& ENDTURN";
char cmd2[16] = "turn\n";
char *str;
int t = 0;
robot.set_auct();
server.send_message(cmd2);
for (;;) {
str = server.read_message();
if (strncmp(str, cmd0, 16)==0) { //in auction table
for (;;) {
delete str;
str = server.read_message();
if ((strncmp(str, "& BOUGHT", 8)==0)||
(strncmp(str, "& SOLD", 6)==0)){
t = 1;
robot.add_new_elem(str);
}
else {
if (t==1) {
break;
}
}
}
} // out auction table
if (strncmp(str, "# You are a bankrupt, sorry.", 28)==0) {
printf("%s", str);
delete str;
robot.free_auct_list();
exit(0);
}
if (strncmp(str, cmd, 9)==0) {
delete str;
break;
}
delete str;
}
return 0;
}
PolizElem* PolizProd::EvaluateFun(PolizItem **stack) const {
int amount;
PolizElem *operand1 = Pop(stack);
PolizInt *i1 = dynamic_cast<PolizInt*>(operand1);
if (!i1) throw PolizExNotInt(operand1);
amount = i1->Get();
char cmd[16] = "prod ", str1[9];
sprintf(str1, "%d\n", amount);
strcat(cmd, str1);
printf("cmd_prod = %s", cmd);
server.send_message(cmd);
delete operand1;
return 0;
}
int main(int argc, char** argv)
{
int mpi_rank = Dune::MPIHelper::instance(argc,argv).rank();
int mpi_size = Dune::MPIHelper::instance(argc,argv).size();
std::cout << "Hello from rank " << mpi_rank << " total mpi size is " << mpi_size << std::endl;
typedef int LocalId;
typedef int GlobalId;
typedef Dune::OwnerOverlapCopyCommunication<LocalId,GlobalId> Communication;
typedef Dune::OwnerOverlapCopyAttributeSet GridAttributes;
typedef GridAttributes::AttributeSet GridFlag;
typedef Dune::ParallelLocalIndex<GridFlag> LocalIndex;
Communication comm;
Communication::PIS& pis = comm.indexSet();
pis.beginResize();
for (int i = 0; i < ltg_size; ++i) {
GridFlag flag = GridAttributes::owner;
bool is_public = false;
pis.add(local_to_global[i], LocalIndex(i, flag, is_public));
}
pis.endResize();
}
void initializeModules(void) {
communication.begin();
//sensor_dfr01610300_water_ph_temperature_ec_default.begin();
sensor_venier_ph_default.begin();
sensor_vernier_ec_default.begin();
sensor_ds18b20_water_temperature.begin();
sensor_tsl2561_light_intensity_default.begin();
sensor_dht22_air_temperature_humidity_default.begin();
sensor_gc0011_air_co2_temperature_humidity_default.begin();
sensor_contact_switch_general_shell_open_default.begin();
sensor_contact_switch_general_window_open_default.begin();
actuator_relay_air_heater_default.begin();
actuator_relay_air_humidifier_default.begin();
actuator_relay_air_vent_default.begin();
actuator_relay_air_circulation_default.begin();
actuator_relay_light_panel_default.begin();
actuator_relay_light_chamber_illumination_default.begin();
actuator_relay_light_motherboard_illumination_default.begin();
// Set Default States
actuator_relay_air_circulation_default.set("AACR", 1, "1");
actuator_relay_light_motherboard_illumination_default.set("ALMI", 1, "1");
actuator_relay_air_vent_default.set("AAVE", 1, "1");
}
PolizElem* PolizBuy::EvaluateFun(PolizItem **stack) const {
int amount, price;
PolizElem *operand1 = Pop(stack);
PolizInt *i1 = dynamic_cast<PolizInt*>(operand1);
if (!i1) throw PolizExNotInt(operand1);
PolizElem *operand2 = Pop(stack);
PolizInt *i2 = dynamic_cast<PolizInt*>(operand2);
if (!i2) throw PolizExNotInt(operand2);
price = i1->Get();
amount = i2->Get();
robot.expected_raw_sold = amount;
robot.expected_raw_price = price;
char cmd[32] = "buy ";
char str1[9], str2[9];
sprintf(str1, "%d\n", price);
sprintf(str2, "%d ", amount);
strcat(cmd, str2);
strcat(cmd, str1);
printf("cmd_buy = %s", cmd);
server.send_message(cmd);
delete operand1;
delete operand2;
return 0;
}
void initializeModules(void) {
EE_Address = 0;
communication.begin();
sensor_water_ph_temperature_ec_default.begin();
sensor_tsl2561_light_intensity_default.begin();
sensor_dht22_air_temperature_humidity_default.begin();
sensor_gc0011_air_co2_temperature_humidity_default.begin();
actuator_relay_air_heater_default.begin();
chamber_illumination_default.begin();
actuator_relay_air_humidifier_default.begin();
actuator_relay_air_vent_default.begin();
actuator_relay_air_circulation_default.begin();
dose_pump1_default.begin();
dose_pump2_default.begin();
dose_pump3_default.begin();
dose_pump4_default.begin();
actuator_relay_airpump_default.begin();
actuator_relay_co2_default.begin();
actuator_relay_watercir_default.begin();
actuator_relay_coolvalve_default.begin();
// Set Default States
actuator_relay_air_circulation_default.set("AACR", 1, "1");
actuator_relay_air_vent_default.set("AAVE", 1, "1");
chamber_illumination_default.set("AWHI", 1, "0");
chamber_illumination_default.set("AUVI", 1, "0");
chamber_illumination_default.set("ARGB", 1, "0,0,0");
dose_pump1_default.set("ADSP", 1, "0");
dose_pump2_default.set("ADSP", 2, "0");
dose_pump3_default.set("ADSP", 3, "0");
dose_pump4_default.set("ADSP", 4, "0");
actuator_relay_airpump_default.set("AAPR", 1, "0");
actuator_relay_co2_default.set("ACOR", 1, "0");
actuator_relay_watercir_default.set("AWCR", 1, "1");
actuator_relay_coolvalve_default.set("AAPR", 1, "1");
}
lldb::thread_result_t
Communication::ReadThread (lldb::thread_arg_t p)
{
Communication *comm = (Communication *)p;
Log *log(lldb_private::GetLogIfAllCategoriesSet (LIBLLDB_LOG_COMMUNICATION));
if (log)
log->Printf ("%p Communication::ReadThread () thread starting...", p);
uint8_t buf[1024];
Error error;
ConnectionStatus status = eConnectionStatusSuccess;
bool done = false;
while (!done && comm->m_read_thread_enabled)
{
size_t bytes_read = comm->ReadFromConnection (buf, sizeof(buf), 5 * TimeValue::MicroSecPerSec, status, &error);
if (bytes_read > 0)
comm->AppendBytesToCache (buf, bytes_read, true, status);
else if ((bytes_read == 0)
&& status == eConnectionStatusEndOfFile)
{
if (comm->GetCloseOnEOF ())
comm->Disconnect ();
comm->AppendBytesToCache (buf, bytes_read, true, status);
}
switch (status)
{
case eConnectionStatusSuccess:
break;
case eConnectionStatusEndOfFile:
done = true;
break;
case eConnectionStatusError: // Check GetError() for details
if (error.GetType() == eErrorTypePOSIX && error.GetError() == EIO)
{
// EIO on a pipe is usually caused by remote shutdown
comm->Disconnect ();
done = true;
}
if (log)
error.LogIfError (log,
"%p Communication::ReadFromConnection () => status = %s",
p,
Communication::ConnectionStatusAsCString (status));
break;
case eConnectionStatusInterrupted: // Synchronization signal from SynchronizeWithReadThread()
// The connection returns eConnectionStatusInterrupted only when there is no
// input pending to be read, so we can signal that.
comm->BroadcastEvent (eBroadcastBitNoMorePendingInput);
break;
case eConnectionStatusNoConnection: // No connection
case eConnectionStatusLostConnection: // Lost connection while connected to a valid connection
done = true;
LLVM_FALLTHROUGH;
case eConnectionStatusTimedOut: // Request timed out
if (log)
error.LogIfError (log,
"%p Communication::ReadFromConnection () => status = %s",
p,
Communication::ConnectionStatusAsCString (status));
break;
}
}
log = lldb_private::GetLogIfAllCategoriesSet (LIBLLDB_LOG_COMMUNICATION);
if (log)
log->Printf ("%p Communication::ReadThread () thread exiting...", p);
comm->m_read_thread_did_exit = true;
// Let clients know that this thread is exiting
comm->BroadcastEvent (eBroadcastBitNoMorePendingInput);
comm->BroadcastEvent (eBroadcastBitReadThreadDidExit);
return NULL;
}
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[])
{
//BITMAP *the_image;
//PALETTE the_palette;
if (allegro_init() != 0)
return 1;
/*if (argc != 2) {
allegro_message("Usage: 'exbitmap filename.[bmp|lbm|pcx|tga]'\n");
return 1;
}*/
install_keyboard();
if (set_gfx_mode(GFX_AUTODETECT_WINDOWED, 640, 480, 0, 0) != 0) {
//if (set_gfx_mode(GFX_AUTODETECT, 640, 480, 0, 0) != 0) {
if (set_gfx_mode(GFX_SAFE, 640, 480, 0, 0) != 0) {
set_gfx_mode(GFX_TEXT, 0, 0, 0, 0);
allegro_message("Unable to set any graphic mode\n%s\n", allegro_error);
return 1;
}
}
///* read in the bitmap file */
//the_image = load_bitmap(argv[1], the_palette);
//if (!the_image) {
// set_gfx_mode(GFX_TEXT, 0, 0, 0, 0);
// allegro_message("Error reading bitmap file '%s'\n", argv[1]);
// return 1;
//}
///* select the bitmap palette */
//set_palette(the_palette);
Communication* comm = new Communication();
// create menu screen.
ScreenManager* mScreenManager= new ScreenManager();
Model* model = new Model();
mScreenManager->Initialize(model);
mScreenManager->ChangeScreen(0);
// force draw;
mScreenManager->Redraw();
Command cmd = NOCOMMAND;
while (cmd != QUIT)
{
cmd = comm->GetCommand();
mScreenManager->HandleCommand(cmd);
}
// /* blit the image onto the screen */
// blit(the_image, screen, 0, 0, (SCREEN_W-the_image->w)/2,
//(SCREEN_H-the_image->h)/2, the_image->w, the_image->h);
// /* destroy the bitmap */
// destroy_bitmap(the_image);
// readkey();
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;
}
int main(int argc, char** argv)
{
full_argc(argc);
Automat a;
int c;
Lexeme_list *list;
c = getchar();
try {
for (;;) {
if (c==EOF) break;
a.feed_char(c);
a.run();
if (a.full_lex())
a.add_new_elem();
if (a.use_prev==0) {
if (c=='\n')
a.inc_nstr();
c = getchar();
}
if (a.is_long()) throw "too long name";
}
}
catch (const char *s) {
printf("Error: %s\n", s);
return 1;
}
a.print_list();
if (a.is_any_errors()) {
printf("Some lexical errors detected!\n");
return 1;
}
list = a.get_list();
PolizItem *lst;
LabelTable *lable_lst;
VarTable *var_lst;
SyntaxAnalyzer s;
s.set_list(list);
try {
s.analyze();
printf("SUCCESSFULL ANALYZATION!!!\n\n");
lst = s.poliz_list;
lable_lst = s.label_list;
}
catch (const Exeption &ex) {
printf("Error: %s\nbut found %s\nstr# %d\n",
ex.GetComment(), ex.GetLexeme(), ex.GetNstr());
return 1;
}
lprint(lst);
server.set(atoi(argv[2]), argv[1]);
server.connection();
robot.set(argv, server);
robot.readnsend_name();
robot.join_or_create();
printf("start playing\n");
robot.start_easy_game(lst);
var_lst = PolizElem::Get_var_table();
delete_var_list(var_lst);
delete_label_list(lable_lst);
delete_poliz_list(lst);
return 0;
}
