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