L
codice arduino mega stabilizzatore
#define BAUDRATE 100000 // oder 100000 115200
//#define BAUDRATE 115200 // oder 100000 115200
#define SERIALPORT Serial // - uncomment this line if using an arduino based board with more than one HW serial port
class BMC_SBUS
{
public:
uint8_t sbusData[25];
int16_t servos[18];
void begin(void);
void Servo(uint8_t ch, int16_t position);
void Send(void);
void Update(void);
private:
uint8_t byte_in_sbus;
uint8_t bit_in_sbus;
uint8_t ch;
uint8_t bit_in_servo;
};
void BMC_SBUS::begin()
{
//intialise private data arrays
//sbus_data is formatted for correct serial output
//note that the actual 11bit sbus data for each channel is embedded across multiple data bytes in a very stange order
//byte 1 and bytes 24 and 25 should be left as is
//the first is a start byte, the last is a stop byte and the second last holds various flags
//servos is the internal per channel position and is more straightforward - one int_16 per channel
uint8_t loc_sbusData[25] = {0x0f, 0x01, 0x04, 0x20, 0x00, 0xff, 0x07, 0x40, 0x00, 0x02, 0x10, 0x80, 0x2c, 0x64, 0x21, 0x0b, 0x59, 0x08, 0x40, 0x00, 0x02, 0x10, 0x80, 0x00, 0x00};
int16_t loc_servos[18] = {1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 0, 0};
//setup serial port to transmit at 100k baud and use 1 parity and 2 stop bits
SERIALPORT.begin(BAUDRATE, SERIAL_8E2);
//setup public data arrays
memcpy(sbusData, loc_sbusData, 25);
memcpy(servos, loc_servos, 18);
}
void BMC_SBUS::Servo(uint8_t ch, int16_t position)
{
//set servo position on single channel
if ((ch > 0) && (ch <= 16))
{
constrain (position, 0, 2048); //keep within min/max values
servos[ch - 1] = position; //expects a non zero starting index to the channel
}
}
void BMC_SBUS::Send(void)
{
//send data over serial port
SERIALPORT.write(sbusData, 25); //according to docs for Serial we can send the array along as is without a loop
}
void BMC_SBUS::Update(void)
{
//update positions for all servo channels within the SBUS data frame
//ignores digital servos and any failsafe mode stuff that was originally written
//clear out existing sbus data for all channel data bytes
//ignores first and last bytes in the array (start and stop bytes)
//mapping loop relies on initial 0 values - do not omit this step!
uint8_t i;
for (i = 1; i < 24; i++)
{
sbusData[i] = 0;
}
//reset counters
ch = 0;
bit_in_servo = 0;
byte_in_sbus = 1;
bit_in_sbus = 0;
//format sbus data - maps sevo data array to sbus data array 1bit at a time
//correctly deals with the little endian byte order in the process
for (i = 0; i < 176; i++) //16channels*11bits = 176bits
{
if (servos[ch] & (1 << bit_in_servo)) //bitwise AND to check if the correct servo databit is set to 1
{
sbusData[byte_in_sbus] |= (1 << bit_in_sbus); //bitwise OR sets the correct sbus databit if true
}
//increment bit counters
bit_in_sbus++;
bit_in_servo++;
//if end of sbus byte reset sbus bit counter and increment sbus byte counter
if (bit_in_sbus == 8)
{
bit_in_sbus = 0;
byte_in_sbus++;
}
// if we have reached bit 11 in the servo data increment channel index and reset servo bit counter
if (bit_in_servo == 11)
{
bit_in_servo = 0;
ch++;
}
}
}
//Declare BMC_SBUS Object
BMC_SBUS mySBUS;
// Sbus delay value
const int sbusWAIT = 7; //frame timing delay in msecs
// Declare sbus control channels
int panChannel = 1;
int tiltChannel = 2;
int rollChannel = 4;
// Declare Kinowheels Stuff
int XA_SIG = 0, XB_SIG = 1, YA_SIG = 0, YB_SIG = 1, pulsesX, pulsesY;
// Declare Stuff for calculating Speed
int xStampEnd = 0, yStampEnd = 0, timeStampEnd = 0, xPassed, yPassed, timePassed, sendX = 1023, sendY = 1023;
void setup() {
// Serial.begin(100000); überflüssig, weil in MC_SBUS enthalten
// Start KinoWheels Stuff
attachInterrupt(0, XA_RISE, RISING); // Pin 2
attachInterrupt(1, XB_RISE, RISING); // Pin 3
attachInterrupt(4, YA_RISE, RISING); // Pin 19
attachInterrupt(5, YB_RISE, RISING); // Pin 18
// Start BMC_SBUS object
mySBUS.begin();
}
void loop() {
for (int i = 0; i < 1; i++) { //SBUS needs data every 7 Milliseconds. I repeat it three times for some time to pass for calculating speeds.
mySBUS.Servo(tiltChannel, sendY);
mySBUS.Servo(panChannel, sendX);
// Update SBUS object and send data
mySBUS.Update();
mySBUS.Send();
delay(sbusWAIT);
}
timePassed = millis() - timeStampEnd;
xPassed = xStampEnd - pulsesX;
yPassed = pulsesY - yStampEnd;
sendX = 1023 + 100 * xPassed / timePassed;
sendY = 1023 + 100 * yPassed / timePassed;
for (int i = 0; i < 1; i++) { //Maybe this one is not needed. Will find it out later
mySBUS.Servo(tiltChannel, sendY);
mySBUS.Servo(panChannel, sendX);
// Update SBUS object and send data
mySBUS.Update();
mySBUS.Send();
delay(sbusWAIT);
}
xStampEnd = pulsesX;
yStampEnd = pulsesY;
timeStampEnd = millis();
}
//Rotary Encoder Stuff by KinoWheels
void XA_RISE() {
detachInterrupt(0);
//delay(1);
XA_SIG = 1;
if (XB_SIG == 0)
pulsesX++;//moving forward
if (XB_SIG == 1)
pulsesX--;//moving reverse
attachInterrupt(0, XA_FALL, FALLING);
}
void XA_FALL() {
detachInterrupt(0);
//delay(1);
XA_SIG = 0;
if (XB_SIG == 1)
pulsesX++;//moving forward
if (XB_SIG == 0)
pulsesX--;//moving reverse
attachInterrupt(0, XA_RISE, RISING);
}
void XB_RISE() {
detachInterrupt(1);
//delay(1);
XB_SIG = 1;
if (XA_SIG == 1)
pulsesX++;//moving forward
if (XA_SIG == 0)
pulsesX--;//moving reverse
attachInterrupt(1, XB_FALL, FALLING);
}
void XB_FALL() {
detachInterrupt(1);
//delay(1);
XB_SIG = 0;
if (XA_SIG == 0)
pulsesX++;//moving forward
if (XA_SIG == 1)
pulsesX--;//moving reverse
attachInterrupt(1, XB_RISE, RISING);
}
void YA_RISE() {
detachInterrupt(4);
//delay(1);
YA_SIG = 1;
if (YB_SIG == 0)
pulsesY++;//moving forward
if (YB_SIG == 1)
pulsesY--;//moving reverse
attachInterrupt(4, YA_FALL, FALLING);
}
void YA_FALL() {
detachInterrupt(4);
//delay(1);
YA_SIG = 0;
if (YB_SIG == 1)
pulsesY++;//moving forward
if (YB_SIG == 0)
pulsesY--;//moving reverse
attachInterrupt(4, YA_RISE, RISING);
}
void YB_RISE() {
detachInterrupt(5);
//delay(1);
YB_SIG = 1;
if (YA_SIG == 1)
pulsesY++;//moving forward
if (YA_SIG == 0)
pulsesY--;//moving reverse
attachInterrupt(5, YB_FALL, FALLING);
}
void YB_FALL() {
detachInterrupt(5);
//delay(1);
YB_SIG = 0;
if (YA_SIG == 0)
pulsesY++;//moving forward
if (YA_SIG == 1)
pulsesY--;//moving reverse
attachInterrupt(5, YB_RISE, RISING);
}
