Exemplo n.º 1
0
int main()
{
    static const uint8_t outputEnablePin = RPI_GPIO_27;

    Pin pin(outputEnablePin);

    if (pin.init()) {
        pin.setMode(Pin::GpioModeOutput);
        pin.write(0); /* drive Output Enable low */
    } else {
        fprintf(stderr, "Output Enable not set. Are you root?\n");
    }

    PCA9685 pwm;

    pwm.initialize();

    uint16_t R = 0, G = 0, B = 4095;

    pwm.setPWM(2, R);
    pwm.setPWM(1, G);
    pwm.setPWM(0, B);
    printf("LED is yellow\n");
    sleep(1);

    while (true) {
        for (R = 0; R < 4095; R++)
            pwm.setPWM(2, R);
        printf("LED is green\n");
        sleep(1);

        for (B = 4095; B > 0; B--)
            pwm.setPWM(0, B);
        printf("LED is cyan\n");
        sleep(1);

        for (G = 0; G < 4095; G++)
            pwm.setPWM(1, G);
        printf("LED is blue\n");
        sleep(1);

        for (R = 4095; R > 0; R--)
            pwm.setPWM(2, R);
        printf("LED is magneta\n");
        sleep(1);

        for (B = 0; B < 4095; B++)
            pwm.setPWM(0, B);
        printf("LED is red\n");
        sleep(1);

        for (G = 4095; G > 0; G--)
            pwm.setPWM(1, G);
        printf("LED is yellow\n");
        sleep(1);

    }

    return 0;
}
Exemplo n.º 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();
}
Exemplo n.º 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");

}