Esempio 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;
}
Esempio n. 2
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?");
    }

    PCA9685 pwm;

    pwm.initialize();
    pwm.setFrequency(50);

    for(int i = 3; i < 13; i++){
        pwm.setPWMmS(i, SERVO_MAX);
        sleep(1);
        pwm.setPWMmS(i, SERVO_MIN);
        sleep(1);
    }

    return 0;
}
Esempio n. 3
0
int main() {
    const uint8_t ADDRESS = 0x40;
    const uint8_t BUS_NUM = 1;
    PCA9685 driver = PCA9685();
    driver.init(BUS_NUM, ADDRESS);
    driver.setPWMFreq(45);

    Servo* servos[3];
    servos[0] = new Servo(0, &driver/*, -90, 90*/);
    servos[1] = new Servo(1, &driver, -90, 90);
    servos[2] = new Servo(2, &driver, -90, 90);

    bool b = true;

    //servos[0]->setAngle(90, 1000);
    //servos[1]->setAngle(90, 1000);
    //servos[2]->setAngle(90, 1000);

    int s;
    int i = 0;
    int t = 0;
    int val = 0;

    while (b) {
        cout << "Command (1 = setangle  2 = walk): " << endl;
        cin >> s;
        switch (s) {
            case 1:
                cout << "Index: ";
                cin >> i;
                cout << " Angle: ";
                cin >> val;
                cout << " Time: ";
                cin >> t;
                servos[i]->setAngle(val, t);
                cout << endl << "Set " << i << " to " << val << " in " << t << "." << endl;
                break;
            case 2:
                cout << "Not dun yet." << endl;
                break;
            case 3:
                b = false;
                cout << "Stopping" << endl;
                break;
            default:
                cout << "Invalid." << endl;
                break;
        }

    }

    for (int i = 0; i < 3; i++) {
        delete servos[i];
    }

    return 0;
}
Esempio n. 4
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]);

}
Esempio n. 5
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();
}
Esempio n. 6
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");

}