Ok, finally with this code I can control each motor, but the use of a thread for this purpose I think is inefficient.
Is there a better idea?
#include <Common/MS5611.h>
#include <Common/Util.h>
#include <unistd.h>
#include <stdio.h>
#include <Navio2/PWM.h>
#include <Navio2/RCOutput_Navio2.h>
#include <Navio+/RCOutput_Navio.h>
#include <pthread.h>
#include
#define PWM_OUTPUT1 2
#define PWM_OUTPUT2 1
#define PWM_OUTPUT3 3
#define PWM_OUTPUT4 0
std::unique_ptr get_rcout()
{
if( get_navio_version() == NAVIO2 )
{
auto ptr = std::unique_ptr { new RCOutput_Navio2() };
return ptr;
}
else
{
auto ptr = std::unique_ptr { new RCOutput_Navio() };
return ptr;
}
}
auto pwm = get_rcout();
double PWM1 = 0, PWM2 = 0, PWM3 = 0, PWM4 = 0;
void *PWM_Refresh( void *barom )
{
while (true)
{
pwm->set_duty_cycle( PWM_OUTPUT1, PWM1 );
pwm->set_duty_cycle( PWM_OUTPUT2, PWM2 );
pwm->set_duty_cycle( PWM_OUTPUT3, PWM3 );
pwm->set_duty_cycle( PWM_OUTPUT4, PWM4 );
//sleep(0.5);
}
pthread_exit(NULL);
}
int main()
{
MS5611 baro;
if ( check_apm() )
{
return 1;
}
if( !(pwm->initialize(PWM_OUTPUT1)) || !(pwm->initialize(PWM_OUTPUT2)) || !(pwm->initialize(PWM_OUTPUT3)) || !(pwm->initialize(PWM_OUTPUT4)) )
{
return 1;
}
pwm->set_frequency( PWM_OUTPUT1, 50 );
pwm->set_frequency( PWM_OUTPUT2, 50 );
pwm->set_frequency( PWM_OUTPUT3, 50 );
pwm->set_frequency( PWM_OUTPUT4, 50 );
if( !(pwm->enable(PWM_OUTPUT1)) || !(pwm->enable(PWM_OUTPUT2)) || !(pwm->enable(PWM_OUTPUT3)) || !(pwm->enable(PWM_OUTPUT4)) )
{
return 1;
}
pthread_t pwm_thread;
if( pthread_create( &pwm_thread, NULL, PWM_Refresh, (void*)&baro) )
{
printf(“Error: Failed to create barometer thread\n”);
return 0;
}
PWM1 = 1000;
PWM2 = 1000;
PWM3 = 1000;
PWM4 = 1000;
sleep(3);
PWM1 = 1150;
sleep(3);
PWM1 = 1000;
sleep(1);
PWM2 = 1150;
sleep(3);
PWM2 = 1000;
sleep(1);
PWM3 = 1150;
sleep(3);
PWM3 = 1000;
sleep(1);
PWM4 = 1150;
sleep(3);
PWM4 = 1000;
while(true)
{
}
pthread_exit(NULL);
return 1;
}
I’ve got a short video of the output of this code, how can I upload it?
Also, is there a better way to post code in this forum instead of copy-paste?