Unverified Commit 7ea473a0 authored by Simon Thomas's avatar Simon Thomas
Browse files

Initial commit

parents
CXX ?= g++
NAVIO = ../../Navio
PIGPIO_PATH := $(PIGPIO_PATH)
LIB = -L$(PIGPIO_PATH)
INCLUDES = -I ../../Navio -I$(PIGPIO_PATH)
all:
$(MAKE) -C ../../Navio all
$(CXX) -std=gnu++11 $(INCLUDES) $(LIB) main.cpp -L$(NAVIO) -lnavio -o SBC -lrt -lpthread -lpigpio || $(MAKE) pigpio
clean:
rm -f RCInput
Dossier pour le code source de Servo Brake Control pour le bateau
File added
#include <unistd.h>
#include <cstdio>
#include <cstdlib>
#include "Navio2/PWM.h"
#include "Navio+/RCOutput_Navio.h"
#include <Navio+/RCInput_Navio.h>
#include "Navio2/RCOutput_Navio2.h"
#include <Navio2/RCInput_Navio2.h>
#include <Common/Util.h>
#include <memory>
#define SERVO_MIN 1250 /*mS*/
#define SERVO_MAX 1750 /*mS*/
#define READ_FAILED -1
#define POSITION_INDEX_CMD 1
#define POSITION_INDEX_TRIM1 2
#define POSITION_INDEX_TRIM2 3
#define POSITION_INDEX_TRIM3 4
#define DEFAULT_TRIM 1500
#define CHANNEL_SERVO1 3
#define CHANNEL_SERVO2 4
#define CHANNEL_SERVO3 5
using namespace Navio;
//#ifdef _WIN32
// #include <windows.h>
// void sleep(unsigned milliseconds)
// {
// Sleep(milliseconds);
// }
//#else
// #include <unistd.h>
// void sleep(unsigned milliseconds)
// {
// usleep(milliseconds * 1000); // takes microseconds
// }
//#endif
std::unique_ptr <RCOutput> get_rcout()
{
if (get_navio_version() == NAVIO2)
{
auto ptr = std::unique_ptr <RCOutput>{ new RCOutput_Navio2() };
return ptr;
} else
{
auto ptr = std::unique_ptr <RCOutput>{ new RCOutput_Navio() };
return ptr;
}
}
std::unique_ptr <RCInput> get_rcin()
{
if (get_navio_version() == NAVIO2)
{
auto ptr = std::unique_ptr <RCInput>{ new RCInput_Navio2() };
return ptr;
} else
{
auto ptr = std::unique_ptr <RCInput>{ new RCInput_Navio() };
return ptr;
}
}
int main(int argc, char *argv[])
{
int iChannelCmd = 8;
int iCmdServo1, iCmdServo2, iCmdServo3 = DEFAULT_TRIM;
int iTrimServo1, iTrimServo2, iTrimServo3 = DEFAULT_TRIM;
if(argc != 5)
{
printf("Usage: ./SBC ChannelCmd TrimServo1 TrimServo2 TrimServo3\n");
printf("ChannelCmd entre 5 et 8\n");
printf("Trim autour de "); printf("%d \n",DEFAULT_TRIM);
return 1;
}
if (check_apm()) {
return 1;
}
auto rcin = get_rcin();
rcin->initialize();
auto pwm1 = get_rcout();
auto pwm2 = get_rcout();
auto pwm3 = get_rcout();
//Sécurité sur le bon index de commande des servos
int iTempIndex = atoi(argv[POSITION_INDEX_CMD]);
if(iTempIndex >= 5 && iTempIndex <= 8)
iChannelCmd = iTempIndex;
else {
printf("Erreur sur le channel de commande servo : %d \n", iTempIndex);
return 1;
}
//Sécurité sur les trims
int iTempTrim1 = atoi(argv[POSITION_INDEX_TRIM1]);
if(iTempTrim1 >= 500 && iTempTrim1 <= 2200)
iTrimServo1 = iTempTrim1;
else {
printf("Erreur sur le channel de trim servo %d : %d \n",CHANNEL_SERVO1, iTrimServo1);
return 1;
}
int iTempTrim2 = atoi(argv[POSITION_INDEX_TRIM2]);
if(iTempTrim2 >= 500 && iTempTrim2 <= 2200)
iTrimServo2 = iTempTrim2;
else {
printf("Erreur sur le channel de trim servo %d : %d \n",CHANNEL_SERVO2, iTrimServo2);
return 1;
}
int iTempTrim3 = atoi(argv[POSITION_INDEX_TRIM3]);
if(iTempTrim3 >= 500 && iTempTrim3 <= 2200)
iTrimServo3 = iTempTrim3;
else {
printf("Erreur sur le channel de trim servo %d : %d \n",CHANNEL_SERVO3, iTrimServo3);
return 1;
}
if (getuid()) {
fprintf(stderr, "Not root. Please launch like this: sudo %s\n", argv[0]);
return 1;
}
if( !(pwm1->initialize(CHANNEL_SERVO1)) ) {
printf("Erreur d'initialisation sur le channel de commande servo : %d \n", CHANNEL_SERVO1);
return 1;
}
if( !(pwm2->initialize(CHANNEL_SERVO2)) ) {
printf("Erreur d'initialisation sur le channel de commande servo : %d \n", CHANNEL_SERVO2);
return 1;
}
if( !(pwm3->initialize(CHANNEL_SERVO3)) ) {
printf("Erreur d'initialisation sur le channel de commande servo : %d \n", CHANNEL_SERVO3);
return 1;
}
pwm1->set_frequency(CHANNEL_SERVO1, 50);
pwm2->set_frequency(CHANNEL_SERVO2, 50);
pwm3->set_frequency(CHANNEL_SERVO3, 50);
if ( !(pwm1->enable(CHANNEL_SERVO1)) ) {
printf("Erreur d'activation sur le channel de commande servo : %d \n", CHANNEL_SERVO1);
return 1;
}
if ( !(pwm2->enable(CHANNEL_SERVO2)) ) {
printf("Erreur d'activation sur le channel de commande servo : %d \n", CHANNEL_SERVO2);
return 1;
}
if ( !(pwm3->enable(CHANNEL_SERVO3)) ) {
printf("Erreur d'activation sur le channel de commande servo : %d \n", CHANNEL_SERVO3);
return 1;
}
//Example de pilotage
// while (true) {
// pwm->set_duty_cycle(PWM_OUTPUT, SERVO_MIN);
// sleep(1);
// pwm->set_duty_cycle(PWM_OUTPUT, SERVO_MAX);
// sleep(1);
// }
//Le mal !
while (true)
{
//Lecture commande
int period = rcin->read(iChannelCmd);
if (period == READ_FAILED)
return EXIT_FAILURE;
printf("%d\n", period);
iCmdServo1 = period - DEFAULT_TRIM + iTrimServo1;
iCmdServo2 = period - DEFAULT_TRIM + iTrimServo2;
iCmdServo3 = period - DEFAULT_TRIM + iTrimServo3;
//Ecriture sur servos
pwm1->set_duty_cycle(CHANNEL_SERVO1, iCmdServo1);
pwm2->set_duty_cycle(CHANNEL_SERVO2, iCmdServo2);
pwm3->set_duty_cycle(CHANNEL_SERVO3, iCmdServo3);
//Freq de 100Hz
sleep(1);
}
return 0;
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment