void task_main(int argc, char *argv[])
{
_is_running = true;
if (pwm_initialize(_device) < 0) {
PX4_ERR("Failed to initialize PWM.");
return;
}
// Subscribe for orb topics
_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
// Start disarmed
_armed.armed = false;
_armed.prearmed = false;
// Set up poll topic
px4_pollfd_struct_t fds[1];
fds[0].fd = _controls_sub;
fds[0].events = POLLIN;
/* Don't limit poll intervall for now, 250 Hz should be fine. */
//orb_set_interval(_controls_sub, 10);
// Set up mixer
if (initialize_mixer(MIXER_FILENAME) < 0) {
PX4_ERR("Mixer initialization failed.");
return;
}
pwm_limit_init(&_pwm_limit);
// Main loop
while (!_task_should_exit) {
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10);
/* Timed out, do a periodic check for _task_should_exit. */
if (pret == 0) {
continue;
}
/* This is undesirable but not much we can do. */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);
_outputs.timestamp = _controls.timestamp;
/* do mixing */
_outputs.noutputs = _mixer->mix(_outputs.output,
0 /* not used */,
NULL);
/* disable unused ports by setting their output to NaN */
for (size_t i = _outputs.noutputs;
i < sizeof(_outputs.output) / sizeof(_outputs.output[0]);
i++) {
_outputs.output[i] = NAN;
}
const uint16_t reverse_mask = 0;
uint16_t disarmed_pwm[4];
uint16_t min_pwm[4];
uint16_t max_pwm[4];
for (unsigned int i = 0; i < 4; i++) {
disarmed_pwm[i] = _pwm_disarmed;
min_pwm[i] = _pwm_min;
max_pwm[i] = _pwm_max;
}
uint16_t pwm[4];
// TODO FIXME: pre-armed seems broken
pwm_limit_calc(_armed.armed,
false/*_armed.prearmed*/,
_outputs.noutputs,
reverse_mask,
disarmed_pwm,
min_pwm,
max_pwm,
_outputs.output,
pwm,
&_pwm_limit);
if (_armed.lockdown) {
send_outputs_pwm(disarmed_pwm);
} else {
send_outputs_pwm(pwm);
}
if (_outputs_pub != nullptr) {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
} else {
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
}
}
bool updated;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
}
}
pwm_deinitialize();
orb_unsubscribe(_controls_sub);
orb_unsubscribe(_armed_sub);
_is_running = false;
}
/*******************************************************************************
* MAIN FUNCTION *
*******************************************************************************/
int main(void)
{
unsigned char angle = 0; // declare a variable to store angle
unsigned char i = 0,j = 0;
unsigned char lx = 0, ly = 0, ry = 0;
unsigned int rx = 0;
// ensure all the hardware port in zero initially
PORTA = 0;
PORTB = 0;
PORTC = 0;
PORTD = 0;
PORTE = 0;
// Initialize the I/O port direction, this must be configured according to circuit
// please refer to PTK40A schematic for details
// TRISX control pin direction, output pin must be configure as '0'
// while input must be configure as '1'
TRISA = 0b00010001;
TRISB = 0b00001111;
TRISC = 0b10010011;
TRISD = 0;
TRISE = 0;
// Initialize ADC.
adc_initialize(); //Ensure pin share with analog is being configured to digital correctly
pwm_initialize();
uart_initialize();
beep(2); //buzzer sound for twice
// PTK40A come SKPS PORT TO USE SKPS
// SKPS, control DC motor speed and RC servo motor
// Please connect SKPS at the "Cytron Starter Kit" section
// Select 9600 as the baud rate on SKPS
// servo require the PCM at around 20ms period
// Please move jumper JP9 to SERVO, JP10 to PWM, JP20 & 21 to DC MOTOR
M1 = 1;
M2 = 0; // drive motor in a direction
while(1) // create an infinite loop
{
lx = uc_skps(p_joy_lx); //obtain left joystick x axis value
ly = uc_skps(p_joy_ly); //obtain left joystick y axis value
rx = uc_skps(p_joy_rx); //obtain right joystick x axis value
ry = uc_skps(p_joy_ry); //obtain right joystick y axis value
//control dc motor using joyleft
if(lx==128 && ly==128)
{
pwm_set_duty_cycle(0);
}
//if(lx!=128 && ly!=128)
else if(ly==0)
{
pwm_set_duty_cycle(1000); //set dc motor speed in differect coordinate
}
else if(lx==0)
{
pwm_set_duty_cycle(350);
}
else if(ly==255)
{
pwm_set_duty_cycle(500);
}
else if(lx==255)
{
pwm_set_duty_cycle(750);
}
//control servo motor using joyright
if(ry==0)
{
SERVO = 1; // Servo pin HIGH
delay_10us(100);
SERVO = 0; // Servo pin LOW
delay_ms(18); // delay for around 18ms
}
else if(rx==0)
{
SERVO = 1; // Servo pin HIGH
delay_10us(130);
SERVO = 0; // Servo pin LOW
delay_ms(18); // delay for around 18ms
}
else if(ry==255)
{
SERVO = 1; // Servo pin HIGH
delay_10us(160);
SERVO = 0; // Servo pin LOW
delay_ms(18); // delay for around 18ms
}
else if(rx==255)
{
SERVO = 1; // Servo pin HIGH
delay_10us(200);
SERVO = 0; // Servo pin LOW
delay_ms(18); // delay for around 18ms
}
}
while(1) continue; // infinite loop to prevent PIC from reset if there is no more program
}
