void Control::setup(){
int fd = pca9685Setup(PIN_BASE, 0x40, HERTZ);
if (fd < 0)
{
printf("Error in setup\n");
}
// Reset all output
pca9685PWMReset(fd);
adjustYPMotor(0);
adjustXPMotor(0);
adjustYNMotor(0);
adjustXNMotor(0);
delay(1000);
std::cout<< "Ready for test..."<<std::endl;
}
int main(void)
{
printf("PCA9685 servo example\n");
printf("Connect a servo to any pin. It will rotate to random angles\n");
// Setup with pinbase 300 and i2c location 0x40
int fd = pca9685Setup(PIN_BASE, 0x40, HERTZ);
if (fd < 0)
{
printf("Error in setup\n");
return fd;
}
// Reset all output
pca9685PWMReset(fd);
// Set servo to neutral position at 1.5 milliseconds
// (View http://en.wikipedia.org/wiki/Servo_control#Pulse_duration)
float millis = 1.5;
int tick = calcTicks(millis, HERTZ);
pwmWrite(PIN_BASE + 16, tick);
delay(2000);
int active = 1;
while (active)
{
// That's a hack. We need a random number < 1
float r = rand();
while (r > 1)
r /= 10;
millis = map(r, 1, 2);
tick = calcTicks(millis, HERTZ);
pwmWrite(PIN_BASE + 16, tick);
delay(1000);
}
return 0;
}
int main(void)
{
printf("PCA9685 LED example\n");
// Calling wiringPi setup first.
wiringPiSetup();
int fd = pca9685Setup(PIN_BASE, 0x40, HERTZ);
if (fd < 0)
{
printf("Error in setup\n");
return fd;
}
pca9685PWMReset(fd);
int i, j;
int active = 1;
while (active)
{
for (j = 0; j < 5; j++)
{
for (i = 0; i < MAX_PWM; i += 32)
{
pwmWrite(PIN_BASE + 16, i);
delay(4);
}
for (i = 0; i < MAX_PWM; i += 32)
{
pwmWrite(PIN_BASE + 16, MAX_PWM - i);
delay(4);
}
}
pwmWrite(PIN_BASE + 16, 0);
delay(500);
for (j = 0; j < 5; j++)
{
for (i = 0; i < 16; i++)
{
pwmWrite(PIN_BASE + i, MAX_PWM);
delay(20);
}
for (i = 0; i < 16; i++)
{
pwmWrite(PIN_BASE + i, 0);
delay(20);
}
}
pwmWrite(PIN_BASE + 16, 0);
delay(500);
}
return 0;
}
int main()
{
pthread_t mpu6050,transport;
int ret;
unsigned int TimeNow,TimeStart;
if (-1 == wiringPiSetup())
{
printf("Setup WiringPi failed!");
return 1;
}
delay(100);
ret = pthread_create(&mpu6050,NULL,gyro_acc,NULL);
if(ret!=0)
{
printf ("Create mpu6050 thread error!\n");
exit (1);
}
TimeStart = millis();
delay(50);
mpu.setI2CMasterModeEnabled(false);//不知道这句话要放哪，此处有作用
mpu.setI2CBypassEnabled(true);
int fd_pca9685 = pca9685Setup(PIN_BASE, 0x40, HERTZ);
if (fd_pca9685 < 0)
{
printf("Error in setup pca9685\n");
return 0;
}
pca9685PWMReset(fd_pca9685);
/***********
//启动方法1：最高油门确认
PWMOut(PinNumber1,0.99);
PWMOut(PinNumber2,0.99);
PWMOut(PinNumber3,0.99);
PWMOut(PinNumber4,0.99);
printf("Way1:input to start ");
getchar();
PWMOut(PinNumber1,0.02);
PWMOut(PinNumber2,0.02);
PWMOut(PinNumber3,0.02);
PWMOut(PinNumber4,0.02);
delay(1200);
PWMOut(PinNumber1,0.05);
PWMOut(PinNumber2,0.05);
PWMOut(PinNumber3,0.05);
PWMOut(PinNumber4,0.05);
printf("start!");
fflush(stdout);
***************/
//启动方法2：最低油门拉起
printf("Way 2:PWM in 0 \n");
PWMOut(PinNumber1,0);
PWMOut(PinNumber2,0);
PWMOut(PinNumber3,0);
PWMOut(PinNumber4,0);
printf("input to start!\n");
fflush(stdout);
getchar();
START_FLAG = 1;
PWMOut(PinNumber1,0.06);
PWMOut(PinNumber2,0.06);
PWMOut(PinNumber3,0.06);
PWMOut(PinNumber4,0.06);
delay(500);
/*********************/
getchar();
ret = pthread_create(&transport,NULL,KeyBoard,NULL);
if(ret!=0)
{
printf ("Create KeyBoard thread error!\n");
exit (1);
}
while(1)
{
//Pid_Pitch = PidUpdate(Angle[1],-1.6,AngleSpeed[1]);
//Pid_Roll = PidUpdate_roll(Angle[0],0,AngleSpeed[0]);
TimeNow = millis();
system("clear");
//delay(100);
printf("Pid_Roll:%.4f error %.3f All_Count: %d time:%d\n",Pid_Roll,error,All_Count,TimeNow - TimeStart);
printf("A:%.2f %.2f %.2f\n",Angle[0],Angle[1],Angle[2]);
printf("Default_Acc:%.2f gyro：Pitch：%.2f roll :%.2f\n",Default_Acc,AngleSpeed[1],AngleSpeed[0]);
fflush(stdout);
//DutyCycle[3] = Default_Acc + Pid_Pitch - Pid_Roll;//+yaw
//DutyCycle[2] = Default_Acc - Pid_Pitch + Pid_Roll;//+yaw
//DutyCycle[1] = Default_Acc + Pid_Pitch + Pid_Roll;//-yaw
//DutyCycle[0] = Default_Acc - Pid_Pitch - Pid_Roll;//-yaw
}
}
int main()
{
pthread_t mpu6050,transport;
int ret;
unsigned int TimeNow,TimeStart;
Pid_Inital();
if (-1 == wiringPiSetup())
{
printf("Setup WiringPi failed!");
return 1;
}
delay(100);
ret = pthread_create(&mpu6050,NULL,gyro_acc,NULL);
if(ret!=0)
{
printf ("Create mpu6050 thread error!\n");
exit (1);
}
TimeStart = millis();
delay(50);
mpu.setI2CMasterModeEnabled(false);//不知道这句话要放哪，此处有作用
mpu.setI2CBypassEnabled(true);
int fd_pca9685 = pca9685Setup(PIN_BASE, 0x40, HERTZ);
if (fd_pca9685 < 0)
{
printf("Error in setup pca9685\n");
return 0;
}
pca9685PWMReset(fd_pca9685);
/***********
//启动方法1：最高油门确认
PWMOut(PinNumber1,0.99);
PWMOut(PinNumber2,0.99);
PWMOut(PinNumber3,0.99);
PWMOut(PinNumber4,0.99);
printf("Way1:input to start ");
getchar();
PWMOut(PinNumber1,0.02);
PWMOut(PinNumber2,0.02);
PWMOut(PinNumber3,0.02);
PWMOut(PinNumber4,0.02);
delay(1200);
PWMOut(PinNumber1,0.05);
PWMOut(PinNumber2,0.05);
PWMOut(PinNumber3,0.05);
PWMOut(PinNumber4,0.05);
printf("start!");
fflush(stdout);
***************/
//启动方法2：最低油门拉起
printf("Way 2:PWM in 0 \n");
PWMOut(PinNumber1,0);
PWMOut(PinNumber2,0);
PWMOut(PinNumber3,0);
PWMOut(PinNumber4,0);
printf("input to start!\n");
fflush(stdout);
getchar();
START_FLAG = 1;
PWMOut(PinNumber1,0.06);
PWMOut(PinNumber2,0.06);
PWMOut(PinNumber3,0.06);
PWMOut(PinNumber4,0.06);
delay(500);
/*********************/
ret = pthread_create(&transport,NULL,KeyBoard,NULL);
if(ret!=0)
{
printf ("Create KeyBoard thread error!\n");
exit (1);
}
while(1)
{
TimeNow = millis();
system("clear");
printf("Pid_Roll:%.4f pid_error:%.3f pid_pError:%.3f pregyro %.3f All_Count: %d",Pid_Roll,pid_error,Roll_PError,pregyro,All_Count);
printf("A:%.2f %.2f %.2f\n",Angle[0],Angle[1],Angle[2]);
printf("Default_Acc:%.2f gyro： roll :%.2f\n",Default_Acc,AngleSpeed[0]);
fflush(stdout);
}
}
int main(void)
{
printf("PCA9685 User-defined signal generation for KK2\n");
// Setup with pinbase 300 and i2c location 0x40
int fd = pca9685Setup(PIN_BASE, 0x40, 1/PERIOD);
if (fd < 0)
{
printf("Error in setup\n");
return fd;
}
// Reset all output
pca9685PWMReset(fd);
int cmd = 1;
float dutytime = MIN_DUTY;
int dutyTicks = 0;
int channel = 0;
while(cmd != 0){
printMenu();
printf("Select an option: ");
cmd = getchar()-'0';
switch(cmd){
case 1:
printf("Select channel: ");
getchar();
channel = getchar()-'0';
printf("Testing 1.25ms...\n");
//setPWMSignal(channel, PERIOD, 0.0);
//dutytime = map(0.5, , 1.25);
dutyTicks = calcTicks(1.25, PERIOD);
printf("dutyTicks = %d\n", dutyTicks);
pwmWrite(PIN_BASE, dutyTicks);
delay(2000);
printf("Testing 1.50ms...\n");
//dutytime = map(0, 1.5, 1.5);
//setPWMSignal(channel, PERIOD, 0.5);
dutyTicks = calcTicks(1.50, PERIOD);
printf("dutyTicks = %d\n", dutyTicks);
pwmWrite(PIN_BASE, dutyTicks);
delay(2000);
printf("Testing 1.75ms...\n");
//dutytime = map(0, 1.75, 1.75);
//setPWMSignal(channel, PERIOD, 1.0);
dutyTicks = calcTicks(1.75, PERIOD);
printf("dutyTicks = %d\n", dutyTicks)
pwmWrite(PIN_BASE, dutyTicks);
delay(2000);
break;
case 2:
break;
default:
break;
}
}
// Set servo to neutral position at 1.5 milliseconds
// (View http://en.wikipedia.org/wiki/Servo_control#Pulse_duration)
/*float millis = 1.5;
int tick = calcTicks(millis, HERTZ);
pwmWrite(PIN_BASE, tick);
delay(2000);
int active = 1;
while (active)
{
// That's a hack. We need a random number < 1
float r = rand() /(rand() * 10);
//while (r > 1)
// r /= 10;
printf("r: %lf\n", r);
millis = map(r, 1.25, 1.75);
printf("millis: %lf\n", millis);
tick = calcTicks(millis, HERTZ);
printf("tick: %d\n", tick);
pwmWrite(PIN_BASE, tick);
delay(1000);
}*/
return 0;
}