void MainTCPserver::create_getstate_json() //NOT READY
{
//int MotorState[]={4001,4002,4003,4004};
char MainPowerState[6];
MySensor.Altitude=23;
if(mypwm.GetMainPower())
{
sprintf(MainPowerState,"true");
}
else
{
sprintf(MainPowerState,"false");
}
sprintf(TX_buffer,"{\"MessageType\":\"GetState\",\"GyroX\":%7.3f,\"GyroY\":%7.3f,\"GyroZ\":%7.3f,\"AccelX\":%7.3f,\"AccelY\":%7.3f,\"AccelZ\":%7.3f,\"KalmanXangle\":%7.3f,\"KalmanYangle\": %7.3f,\"Altitude\": %d,\"MainPowerState\": \"%s\",\"MotorState0\": %d,\"MotorState1\":%d,\"MotorState2\":%d,\"MotorState3\":%d}",MySensor.GData.x,MySensor.GData.y,MySensor.GData.z,MySensor.AData.x,MySensor.AData.y,MySensor.AData.z,MySensor.KFData.x,MySensor.KFData.y,MySensor.Altitude,MainPowerState,mypwm.MotorStateArray[0],mypwm.MotorStateArray[1],mypwm.MotorStateArray[2],mypwm.MotorStateArray[3]);
}
int main (int argc, char** argv)
{
if (argc != 2) { //checking the validity of the input parameters
printf("usage: %s <port>\n", argv[0]);
exit(1);
}
int count_dir=0;
Timer T; //time
int i=0;
mypwm.init(1,0x40);
mypwm.StartMotors();
//mypwm.setPWM(0,2000);
MainTCPserver TCPserver(atoi(argv[1]));
TCPserver.start_listening();
printf("the main loop has started\n");
while(1)
{
T.StartCycle(); //start counting the time
MySensor.KalmanFiltering(); //reading IMU datas, and Kalman filtering
if(i==10)
{
TCPserver.report_state(); //send state report
i=0;
}
T.CountElapsedTime(); //stop counting time
//printf("Time: %f ms motor: %d ",T.elapsedTime,mypwm.MotorStateArray[0]);
T.WaitMs(dt*1000);//wait 10ms, dt is defined in the IMU library
if(mypwm.GetMainPower())
{
if(mypwm.MotorStateArray[0] == 2481)
{
count_dir=1;
}
if(mypwm.MotorStateArray[0] == 1300)
{
count_dir=0;
}
if(count_dir==0)
{
mypwm.setPWM(0,mypwm.MotorStateArray[0]+1);
}
if(count_dir==1)
{
mypwm.setPWM(0,mypwm.MotorStateArray[0]-1);
}
}
i++;
}
TCPserver.stop_listening();
}
void* MainTCPserver::proc_rx(void)
{
if(strlen(RX_buffer)<1)
{
return 0;
}
bzero(TX_buffer,300);
Document InputJSON;
int len;
//sprintf(RX_buffer,"{\"MessageType\":\"SetPID\"}");
if(InputJSON.Parse<0>(RX_buffer).HasParseError()) //convert string to JSON obj
{
printf("ERROR in parsing JSON str\n");
}
if(InputJSON["MessageType"] == "GetState")
{
printf("GetState message detected\n");
create_getstate_json();
}
else if(InputJSON["MessageType"] == "GetPID")
{
printf("GetPID message detected\n");
create_getpid_json();
}
else if(InputJSON["MessageType"] == "SetPID")
{
printf("SetPID message detected\n");
assert(InputJSON["Kp"].IsNumber());
assert(InputJSON["Ki"].IsNumber());
assert(InputJSON["Kd"].IsNumber());
Kp=InputJSON["Kp"].GetInt();
Ki=InputJSON["Ki"].GetInt();
Kd=InputJSON["Kd"].GetInt();
create_setpid_json(true);
}
else if(InputJSON["MessageType"] == "SetMainPower")
{
printf("SetMainPower message detected\n");
if(InputJSON["MainPowerStatus"] == "true")//turn on the motors
{
printf("Set power to high is detected!\n");
if(mypwm.GetMainPower() == false)
{
mypwm.SetMainPower(true);
//mypwm.StartMotors();
}
printf("it was high already!!!\n");
}
else if(InputJSON["MainPowerStatus"] == "false")
{
printf("Set power to low is detected!\n");
mypwm.SetMainPower(false);
printf("1\n");
mypwm.setPWM(0,1240);
printf("2\n");
}
create_setmainpower_json(mypwm.GetMainPower());
}
else
{
sprintf(TX_buffer,"{\"MessageType\":\"ERROR\"}");
}
write(comm_fd,TX_buffer,strlen(TX_buffer)); //sending the content of TX_buffer to the connected client
printf("kaka");
}