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