void MINI_CONTROL::Send_Order(order_type Type)
{
char data_size;
char * dataput;
switch (Type)
{
case(steer):
data_size=sizeof(short)+1;
dataput=(char*)malloc(data_size+3);
dataput[0]=pack_header;
dataput[1]=data_size;
dataput[2]=0x01;
*(short*)&dataput[3]=-_Default_VOLC.Steer;
break;
case(speed):
data_size=sizeof(short)+1;
dataput=(char*)malloc(data_size+3);
dataput[0]=pack_header;
dataput[1]=data_size;
dataput[2]=0x02;
*(short*)&dataput[3]=_Default_VOLC.Velocity;
break;
case(speedPID):
data_size=sizeof(short)*4;
dataput=(char*)malloc(data_size+3);
dataput[0]=pack_header;
dataput[1]=data_size;
*(short*)&dataput[2]=_Default_VOLC.Kp;
*(short*)&dataput[4]=_Default_VOLC.Ki;
*(short*)&dataput[6]=_Default_VOLC.Kd;
*(short*)&dataput[8]=_Default_VOLC.Velocity;
break;
case(both):
data_size=sizeof(short)*2;
dataput=(char*)malloc(data_size+3);
dataput[0]=pack_header;
dataput[1]=data_size;
*(short*)&dataput[2]=_Default_VOLC.Steer;
*(short*)&dataput[4]=_Default_VOLC.Velocity;
break;
default:
return;
break;
}
if(dataput[0]==pack_header){
order_lock.lockForWrite();
send_orders->clear();
send_orders->append(dataput,data_size+2);
send_orders->append(pack_tail);
free(dataput);
emit dataUpdate((void *)send_orders,&order_lock);
order_lock.unlock();
//Serial_Com->write(send_orders->data(),send_orders->size());
}
}
int main(int argc, char *argv[])
{
QApplication a(argc, argv);
NatNetReceiver mocap;
PositionDispatcher positionDispatcher;
//MainControl controller;
Commander commander;
ManualControl manual;
Automatic autom;
Executioner ex;
//Trigger for automatic control
Window *win = new Window(200,200);
win->button->setText("AUTO");
QObject::connect(win->button, SIGNAL(clicked()), &autom.thread, SLOT(startMe()));
QObject::connect(win->button, SIGNAL(clicked()), &ex.thread, SLOT(startMe()));
win->show();
//Connect manual control to commander
QObject::connect(&manual,SIGNAL(publish()),&commander,SLOT(checkCommands()));
QObject::connect(&autom,SIGNAL(publish()),&commander,SLOT(checkCommands()));
qDebug() << "MAIN FROM: " << QThread::currentThreadId();
QObject::connect(&mocap, SIGNAL(dataUpdate()), &positionDispatcher, SLOT(sendPosition()));
QObject::connect(&positionDispatcher, SIGNAL(finished()), &a, SLOT(quit()));
return a.exec();
}
