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