예제 #1
0
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]);

}
예제 #2
0
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();
}
예제 #3
0
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");

}