Beispiel #1
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;
}
Beispiel #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();
}