Dit is mijn meest uitgebreide versie met ook besturing naar de motor toe.
Code: Selecteer alles
/************************************************************************************
*
* Name : Full monocopter control
* Author : Johannes Lubben
* Date : 4 January 2014
* Version : Beta 0.0
* Resources : rcarduino.blogspot.com,
*
* monocopter code for full control of a monocopter in flight, pitch, nick, roll, yaw and motorspeed.
* With motorcontrol and flap for steering.
* callibrate yaw using a potmeter channel on your transmitter.
* based on high speed reading of the Honeywell HMC5883L up to 500Hz.
* Reading multi channels with pin change interrupt.
*
* Copyright (c) 2014 Luppie. All right reserved.
*
*
***********************************************************************************/
#include <PinChangeInt.h>
#include <Wire.h>
#include <Servo.h>
/* define all MAG related stuff */
#define MAG_ADDRESS ((char) 0x1E) //The I2C address of the MAG module
uint8_t mag_buffer[6];
int16_t mag_raw[3];
volatile boolean isDataReady = false;
long start_time = 0;
long previous_time = 0;
long loop_duration = 0;
/*define all pinchange interrupt related stuff*/
#define PITCH_IN_PIN 2 //channel in pins
#define NICK_IN_PIN 3
#define ROLL_IN_PIN 4
#define YAW_IN_PIN 5
#define CALYAW_IN_PIN 6
#define MOTOR_IN_PIN A2
#define SERVO_OUT_PIN 7 //channel out pins
#define MOTOR_OUT_PIN A0 //channel out pins
// Servo objects generate the signals expected by Electronic Speed Controllers and Servos
// We will use the objects to output the signals we read in
// this example code provides a straight pass through of the signal with no custom processing
Servo servoPITCH;
Servo servoNICK;
Servo servoROLL;
Servo servoYAW;
Servo servoCALYAW;
Servo servoMOTOR;
Servo servo_motor_output;
Servo servo;
// These bit flags are set in bUpdateFlagsShared to indicate which
// channels have new signals
#define PITCH_FLAG 1
#define NICK_FLAG 2
#define ROLL_FLAG 4
#define YAW_FLAG 8
#define CALYAW_FLAG 16
#define MOTOR_FLAG 32
// holds the update flags defined above
volatile uint8_t bUpdateFlagsShared;
// shared variables are updated by the ISR and read by loop.
// In loop we immediatley take local copies so that the ISR can keep ownership of the
// shared ones. To access these in loop
// we first turn interrupts off with noInterrupts
// we take a copy to use in loop and the turn interrupts back on
// as quickly as possible, this ensures that we are always able to receive new signals
volatile uint16_t unPITCHInShared;
volatile uint16_t unNICKInShared;
volatile uint16_t unROLLInShared;
volatile uint16_t unYAWInShared;
volatile uint16_t unCALYAWInShared;
volatile uint16_t unMOTORInShared;
// These are used to record the rising edge of a pulse in the calcInput functions
// They do not need to be volatile as they are only used in the ISR. If we wanted
// to refer to these in loop and the ISR then they would need to be declared volatile
uint32_t ulPITCHStart;
uint32_t ulNICKStart;
uint32_t ulROLLStart;
uint32_t ulYAWStart;
uint32_t ulCALYAWStart;
uint32_t ulMOTORStart;
/*difine all stuff related to monocopter calculation */
unsigned long time0= 0; //time for looptime
unsigned int headingDegrees= 0; //vallue in degrees from the MAG
int yawinput= 0; //inputsignal to calculate with
int motorinput= 0; //inputsignal to calculate with
int calyawinput= 0; //inputsignal to calculate with
int nickinput= 0; //inputsignal to calculate with
int rollinput= 0; //inputsignal to calculate with
int newyaw= 0; //temporary vallue for yaw
int yaw= 0; // vallue in degrees that is front
int yawmin= 0; //vallue in degrees under yaw
int yawplus= 0; //vallue in degrees above yaw
int yawdifMIN= 0; //difference between yaw and yawmin
int yawdifPLUS= 0; //difference between yaw and yawplus
int pitch= 0; // flapdifflection for lift
unsigned long cyclestart= 0; // startpoint for cycle time
unsigned long cyclecounter= 1001; // endpoint for cycletime (starts with 1001 for start of loop)
float cyclespeed= 1001; // cycletime
float cyclepart= 0; // part of the cycletime
int nickinneg= 0; // negative of nickinput
int rollinneg= 0; // negative of rollinput
int rollcycle= 0; //cycle vallue for servo
int nickcycle= 0; //cycle vallue for servo
int motornick= 0; // cycle vallue for motor
int motorroll= 0; // cycle vallue for motor
int calpin= 0; // 1= pot input for LED delay
float calfactor= 0; //store last time LED whas on
const int true_yaw= 13; // true yaw LED
const int cal_yaw= 11; // callibrated yaw LED
const int initialyze= 10; // init LED
const int error= 9; // fault LED
const int takeoffspeed= 8; // LED for takeoffspeed
void setup()
{
Serial.begin(115200);
Wire.begin();
Serial.println("serieele communicatie aan");
pinMode(true_yaw, OUTPUT); // uitgang 13 is ledje
pinMode(cal_yaw, OUTPUT); // uitgang 11 is ledje
pinMode(initialyze, OUTPUT); // init LED
pinMode(error, OUTPUT); // fault LED
pinMode(takeoffspeed, OUTPUT); // LED for takeoffspeed
digitalWrite (initialyze, HIGH);
/* Initialise the module */
configMag();
// attach servo objects, these will generate the correct
// pulses for driving Electronic speed controllers, servos or other devices
// designed to interface directly with RC Receivers
servo.attach(SERVO_OUT_PIN);
servo_motor_output.attach(MOTOR_OUT_PIN);
attachInterrupt(12, magDataReady, RISING);
// using the PinChangeInt library, attach the interrupts
// used to read the channels
PCintPort::attachInterrupt(PITCH_IN_PIN, calcPITCH,CHANGE);
PCintPort::attachInterrupt(NICK_IN_PIN, calcNICK,CHANGE);
PCintPort::attachInterrupt(ROLL_IN_PIN, calcROLL,CHANGE);
PCintPort::attachInterrupt(YAW_IN_PIN, calcYAW,CHANGE);
PCintPort::attachInterrupt(CALYAW_IN_PIN, calcCALYAW,CHANGE);
PCintPort::attachInterrupt(MOTOR_IN_PIN, calcMOTOR,CHANGE);
digitalWrite (initialyze, LOW);
}
//interrupt function when mag DRDY pin is brought LOW
void magDataReady() {
isDataReady = true;
}
void loop()
{
time0=micros(); // sla aan het begin van de loop de milliseconden sinds de start van de arduino op voor loopsnelheid
if(isDataReady = true) //read the MAG at highest speed possible
{
readMag();
//Serial.print(mag_raw[0], DEC); Serial.print(",");
//Serial.print(mag_raw[1], DEC); Serial.print(",");
//Serial.print(mag_raw[2], DEC); Serial.print(",");
isDataReady = false;
}
else {
Serial.println("Missed one");
}
// Calculate heading when the magnetometer is level, then correct for signs of axis.
float heading = atan2(mag_raw[2], mag_raw[0]);
//Serial.print(micros()-time0);Serial.println("na MAG");
// for propper north fill in your current declination angle. The declination angle varies seasonally.
float declinationAngle = 0.0457;
heading += declinationAngle;
// Correct for when signs are reversed.
if(heading < 0)
heading += 2*PI;
// Check for wrap due to addition of declination.
if(heading > 2*PI)
heading -= 2*PI;
// Convert radians to degrees for readability. was float,int van gemaakt ivm rekensnelheid
int headingDegrees = heading * 180/M_PI;
// create local variables to hold a local copies of the channel inputs
// these are declared static so that thier values will be retained
// between calls to loop.
static uint16_t unPITCHIn;
static uint16_t unNICKIn;
static uint16_t unROLLIn;
static uint16_t unYAWIn;
static uint16_t unCALYAWIn;
static uint16_t unMOTORIn;
// local copy of update flags
static uint8_t bUpdateFlags;
// check shared update flags to see if any channels have a new signal
if(bUpdateFlagsShared)
{
noInterrupts(); // turn interrupts off quickly while we take local copies of the shared variables
// take a local copy of which channels were updated in case we need to use this in the rest of loop
bUpdateFlags = bUpdateFlagsShared;
// in the current code, the shared values are always populated
// so we could copy them without testing the flags
// however in the future this could change, so lets
// only copy when the flags tell us we can.
if(bUpdateFlags & PITCH_FLAG)
{
unPITCHIn = unPITCHInShared;
}
if(bUpdateFlags & NICK_FLAG)
{
unNICKIn = unNICKInShared;
}
if(bUpdateFlags & ROLL_FLAG)
{
unROLLIn = unROLLInShared;
}
if(bUpdateFlags & YAW_FLAG)
{
unYAWIn = unYAWInShared;
}
if(bUpdateFlags & CALYAW_FLAG)
{
unCALYAWIn = unCALYAWInShared;
}
if(bUpdateFlags & MOTOR_FLAG)
{
unMOTORIn = unMOTORInShared;
}
// clear shared copy of updated flags as we have already taken the updates
// we still have a local copy if we need to use it in bUpdateFlags
bUpdateFlagsShared = 0;
interrupts(); // we have local copies of the inputs, so now we can turn interrupts back on
// as soon as interrupts are back on, we can no longer use the shared copies, the interrupt
// service routines own these and could update them at any time. During the update, the
// shared copies may contain junk. Luckily we have our local copies to work with :-)
}
// do any processing from here onwards
// only use the local values unROLLIn, unPITCHIn and unNICKIn unYAWIn, the shared
// variables unROLLInShared, unPITCHInShared, unNICKInShared are always owned by
// the interrupt routines and should not be used in loop
nickinput = (unNICKIn-1500); // unNICKIn signal stick up 1000, stick down 2000 becomes -500 +500
rollinput = (unROLLIn-1500); // unROLLIn signal stick left 1000, stick right 2000 becomes -500 +500
pitch = map(unPITCHIn, 1000, 2000, 1250, 1750); //pitch output is between 1250 and 1750
yawinput = (unYAWIn-1466); // unYAWIn signal stick left 1000, stick right 2000 becomes -500 +500
calinput = map(unCALYAWIn, 1000, 2000, 0, 40); //unCALYAWIn signal pot meter left 1000, potmeter right 2000 becomes -500 +500
motorinput = (unMOTORIn-1500); //unMOTORIn signal slider down 1000, slider up 2000 becomes -500 +500
yaw = (newyaw + (yawinput/10)); // Flightdirection newyaw + yawinput (-50 tm +50)
newyaw=yaw; //newyaw is equal to yaw so its vallue is wil be increased by yawinput in next loopcycle.
yaw = map(newyaw, -32767, 32767, 0, 359); // keep yaw within 0 to 359 becouse int are -32767 to 32767.
/* In order to make the led burn longer (+10° and -10° from yaw) when it passen headingdegrees.
And to make it work around 0° and 359° the next calculation:
*/
yawmin = (yaw -10); //LED burns at yaw -10°
yawplus = (yaw +10); // LED burns at yaw +10°
yawdifPLUS = (10 - (359 - yaw)); // 10 minus the difference between 359° and yaw (this has to be added to 0°).
yawdifMIN = (10 - yaw ); // 10 minus the difference between 0° and yaw (this has to be subtracted from 359°).
// clockwise (degrees getting lower vallue)
if (yawmin > 349) //when yawmin is under 354° (yaw would be 359° at this piont)
{
yawmin= 359 - yawdifMIN; //yaw becomes 359° - yawdifMIN.
}
if (yawplus > 359) //When yawplus becomes lager then 359°
{
yawplus = yawdifPLUS; //yawplus becomes 0 + yawdifPLUS.
}
//counterclockwise (degrees getting higher vallue)
if (yawmin < 0) //when yawmin is under 0°
{
yawmin= 359 - yawdifMIN; // yawmin becomes 359 - yawdifMIN.
}
if (yawplus < 10) //when yaw is under 0°
{
yawplus= yawdifPLUS; // yawplus becomes 0 + yawdifPLUS.
}
if (headingDegrees >= yawmin && headingDegrees <= yawplus) //is heading between yawmin and yawplus??
{
digitalWrite (true_yaw, HIGH); // Then LED ON
cyclestart =millis(); //start couting cyclespeed
if (cyclecounter>30)
{
cyclespeed = cyclecounter; //cyclespeed needs the vallue cyclecounter has at this piont in loop
}
}
else
{
digitalWrite (true_yaw, LOW); //else LED OFF
}
if ((headingDegrees >= yawmin && headingDegrees <= yawplus)-1) //is heading between yawmin and yawplus MINUS 1°!!
{
cyclecounter =(millis()-cyclestart); //reset the cyclecounter
}
if (cyclespeed < 300) // when the monocopter has takeoffspeed..
{
Calc_cycles(); //nick and roll are calculated
digitalWrite (takeoffspeed, HIGH); //LED is ON
}
else
{
digitalWrite (takeoffspeed, LOW);
}
//callibrate LED so its ON at real heading. adjustable with pot.
calfactor =cyclepart*calyawinput;
// place in cycle where led is on 0 to 100% cyclespeed
if (calfactor>=cyclecounter-cyclepart && calfactor<=cyclecounter+cyclepart)
{
digitalWrite (cal_yaw, HIGH);
}
else
{
digitalWrite (cal_yaw, LOW); //else LED OFF
}
// output for servo pitch added with cyclic control nick and roll
servo.write(pitch + rollcycle + nickcycle);
//Serial.print("unPITCHIn "); Serial.print(unPITCHIn);
// motoroutput signal.
// this output needs to be blocked at 1000 to 2000
servo_motor_output.write(unMOTORIn + motornick + motorroll);
// we are checking to see if the channel value has changed, this is indicated
// by the flags. For the simple pass through we don't really need this check,
// but for a more complex project where a new signal requires significant processing
// this allows us to only calculate new values when we have new inputs, rather than
// on every cycle.
if(bUpdateFlags & PITCH_FLAG)
{
if(servoPITCH.readMicroseconds() != unPITCHIn)
{
servoPITCH.writeMicroseconds(unPITCHIn);
}
}
if(bUpdateFlags & NICK_FLAG)
{
if(servoNICK.readMicroseconds() != unNICKIn)
{
servoNICK.writeMicroseconds(unNICKIn);
}
}
if(bUpdateFlags & ROLL_FLAG)
{
if(servoROLL.readMicroseconds() != unROLLIn)
{
servoROLL.writeMicroseconds(unROLLIn);
}
}
if(bUpdateFlags & YAW_FLAG)
{
if(servoYAW.readMicroseconds() != unYAWIn)
{
servoYAW.writeMicroseconds(unYAWIn);
}
}
if(bUpdateFlags & CALYAW_FLAG)
{
if(servoCALYAW.readMicroseconds() != unCALYAWIn)
{
servoCALYAW.writeMicroseconds(unCALYAWIn);
}
}
if(bUpdateFlags & MOTOR_FLAG)
{
if(servoMOTOR.readMicroseconds() != unMOTORIn)
{
servoMOTOR.writeMicroseconds(unMOTORIn);
}
}
bUpdateFlags = 0;
Output();
//Serial.print(micros()-time0);Serial.println("loopend"); // for reading loopspeed
}
// Output the data down the serial port.
void Output()
{
//Serial.print (unMOTORIn); Serial.print ("unMOTORIn ");
//Serial.print (yaw); Serial.print ("yaw ");
//Serial.print (rollcycle); Serial.print ("rollcycle/2 ");
//Serial.print (headingDegrees); Serial.print ("headingDegrees ");
//Serial.print (nickcycle); Serial.println ("nickcycle/2 ");
//Serial.print (cyclespeed); Serial.println ("cyclespeed ");
//Serial.print (cyclepart); Serial.print ("cyclepart ");
//Serial.print (calfactor); Serial.print ("calfactor ");
//Serial.print(cyclespeed); Serial.println ("cyclespeed");
//Serial.print (motornick); Serial.print ("motornick ");
//Serial.print(motorroll); Serial.println ("motorroll ");
//Serial.print("rollpuls:\t");
//Serial.print (unROLLIn); // geeft de rollpuls weer
//Serial.print("\tcyclestart.\t"); //geeft de tijd weer sinds de eerste omwenteling
//Serial.print(cyclestart);
//Serial.print("\tcyclecounter.\t"); //geeft weer hoe lang de huidige omwenteling loopt
//Serial.print(cyclecounter);
// Serial.print("\tcyclespeed.\t"); //geeft weer hoe lang de vorrige omwenteling duurde
//Serial.println(cyclespeed);
//Serial.print("\tpart.\t"); //geeft weer hoe lang de vorrige omwenteling duurde
//Serial.println(cyclepart);
//Serial.print("\tkompas.\t"); // geeft de aan waar de vleugel zich bevind
//Serial.println(headingDegrees);
// Serial.print("\tinput.\t"); //geeft yawinputsignaal weer
// Serial.print(yawinput);
// Serial.print("\tyaw.\t"); //geeft de voorkant van de monocopter
// Serial.println(yaw);
//Serial.print("\tyawmin.\t"); // geeft weer waar de vleugel zich bevind -10
//Serial.print(yawmin);
//Serial.print("\tyawplus.\t"); // geeft weer waar de vleugel zich bevind +10
//Serial.print(yawplus);
//Serial.print("\tnickinput.\t"); //geeft weer hoe lang de vorrige omwenteling duurde
//Serial.print(nickinput);
//Serial.print("\trollinput.\t"); //geeft één deel van het rollsignaal weer
//Serial.print(rollinput);
//Serial.print("\tnickinneg.\t"); //geeft weer hoe lang de vorrige omwenteling duurde
//Serial.println(nickinneg);
//Serial.print("\trollinneg.\t"); //geeft één deel van het rollsignaal weer
//Serial.println(rollinneg);
}
/* This function will calculate the signal that is needed to steer the clockwise rotating monocopter.
It wil run after the monocopter has reached takeoff speed.
maximum lift needs to take place 270° before commanded flightdirection ie, maximum lift for command nick in direction of yaw at 270° before yaw.
Command Nick backwards maximun lift at 90° before yaw. Command roll right maximun lift at 180°from yaw. Command roll left maximun lift at yaw*/
void Calc_cycles (void)
{
cyclepart = (cyclespeed/40);
nickinneg = map(nickinput, -500, 500, 500, -500); // th+cycleparte inverse of rollinput needed for rollcycle front side
rollinneg = map(rollinput, -500, 500, 500, -500); // the inverse of nickinput needed for nickcycle left side
//first kwart
if (cyclecounter >= 0 && cyclecounter <= ((int (cyclepart*9))+cyclepart)) // if not skip the first 10 calculations
{ //Serial.print("C1 ");
if (cyclecounter>=0 && cyclecounter<=cyclepart)//start of the cycle.This is the first cyclepart
{
rollcycle = rollinneg; //rollinput-negatif has 100% effect
nickcycle = 0.1*nickinput; // nickinput has 10% effect
}
if (cyclecounter>=(int (cyclepart*1))&&cyclecounter<=(int (cyclepart*1))+cyclepart) //the second cyclepart
{
rollcycle = 0.9*rollinneg; //rollinput-negatif has 90% effect
nickcycle = 0.2*nickinput; //nickinput has 20% effect
}
if (cyclecounter>=(int (cyclepart*2))&&cyclecounter<=(int (cyclepart*2))+cyclepart) //the 3rd cyclepart
{
rollcycle = 0.8*rollinneg; //rollinput-negatif has 80% effect
nickcycle = 0.3*nickinput; //nickinput has 30% effect
}
if (cyclecounter>=(int (cyclepart*3))&&cyclecounter<=(int (cyclepart*3))+cyclepart)//the 4th cyclepart
{
rollcycle = 0.7*rollinneg; //rollinput-negatif has 70% effect
nickcycle = 0.4*nickinput; //nickinput has 40% effect
}
if (cyclecounter>=(int (cyclepart*4))&&cyclecounter<=(int (cyclepart*4))+cyclepart)//the 5th cyclepart
{
rollcycle = 0.6*rollinneg; //rollinput-negatif has 60% effect
nickcycle = 0.5*nickinput; //nickinput has 50% effect
}
if (cyclecounter>=(int (cyclepart*5))&&cyclecounter<=(int (cyclepart*5))+cyclepart)//the 6th cyclepart
{
rollcycle = 0.5*rollinneg; //rollinput-negatif has 50% effect
nickcycle = 0.6*nickinput; //nickinput has 60% effect
}
if (cyclecounter>=(int (cyclepart*6))&&cyclecounter<=(int (cyclepart*6))+cyclepart)//the 7th cyclepart
{
rollcycle = 0.4*rollinneg; //rollinput-negatif has 40% effect
nickcycle = 0.7*nickinput; //nickinput has 70% effect
}
if (cyclecounter>=(int (cyclepart*7))&&cyclecounter<=(int (cyclepart*7))+cyclepart)//the 8th cyclepart
{
rollcycle = 0.3*rollinneg; //rollinput-negatif has 30% effect
nickcycle = 0.8*nickinput; //nickinput has 80% effect
}
if (cyclecounter>=(int (cyclepart*8))&&cyclecounter<=(int (cyclepart*8))+cyclepart)//the 9th cyclepart
{
rollcycle = 0.2*rollinneg; //rollinput-negatif has 20% effect
nickcycle = 0.9*nickinput; //nickinput has 90% effect
}
if (cyclecounter>=(int (cyclepart*9))&&cyclecounter<=(int (cyclepart*9))+cyclepart)//the 10th cyclepart
{
rollcycle = 0.1*rollinneg; //rollinput-negatif has 10% effect
nickcycle = nickinput; //nickinput has 100% effect
}
}
// second kwart starts here
if (cyclecounter >= (int (cyclepart*10)) && cyclecounter <= ((int (cyclepart*19))+cyclepart)) // if not skip the next 10 calculations
{ //Serial.print("C2 ");
if (cyclecounter>=(int (cyclepart*10))&&cyclecounter<=(int (cyclepart*10))+cyclepart)//the 11th cyclepart
{
rollcycle = 0.1*rollinput; //rollinput has 10% effect
nickcycle = nickinput; //nickinput has 100% effect
}
if (cyclecounter>=(int (cyclepart*11))&&cyclecounter<=(int (cyclepart*11))+cyclepart)//the 12th cyclepart
{
rollcycle = 0.2*rollinput; //rollinput has 20% effect
nickcycle = 0.9*nickinput; //nickinput has 90% effect
}
if (cyclecounter>=(int (cyclepart*12))&&cyclecounter<=(int (cyclepart*12))+cyclepart)//the 13th cyclepart
{
rollcycle = 0.3*rollinput; //rollinput has 30% effect
nickcycle = 0.8*nickinput; //nickinput has 80% effect
}
if (cyclecounter>=(int (cyclepart*13))&&cyclecounter<=(int (cyclepart*13))+cyclepart)//the 14th cyclepart
{
rollcycle = 0.4*rollinput; //rollinput has 40% effect
nickcycle = 0.7*nickinput; //nickinput has 70% effect
}
if (cyclecounter>=(int (cyclepart*14))&&cyclecounter<=(int (cyclepart*14))+cyclepart)//the 15th cyclepart
{
rollcycle = 0.5*rollinput; //rollinput has 50% effect
nickcycle = 0.6*nickinput; //nickinput-negatif has 60% effect
}
if (cyclecounter>=(int (cyclepart*15))&&cyclecounter<=(int (cyclepart*15))+cyclepart)//the 16th cyclepart
{
rollcycle = 0.6*rollinput; //rollinput has 60% effect
nickcycle = 0.5*nickinput; //nickinput has 50% effect
}
if (cyclecounter>=(int (cyclepart*16))&&cyclecounter<=(int (cyclepart*16))+cyclepart)//the 17th cyclepart
{
rollcycle = 0.7*rollinput; //rollinput has 70% effect
nickcycle = 0.4*nickinput; //nickinput has 40% effect
}
if (cyclecounter>=(int (cyclepart*17))&&cyclecounter<=(int (cyclepart*17))+cyclepart)//the 18th cyclepart
{
rollcycle = 0.8*rollinput; //rollinput has 80% effect
nickcycle = 0.3*nickinput; //nickinput has 30% effect
}
if (cyclecounter>=(int (cyclepart*18))&&cyclecounter<=(int (cyclepart*18))+cyclepart)//the 19th cyclepart
{
rollcycle = 0.9*rollinput; //rollinput has 90% effect
nickcycle = 0.2*nickinput; //nickinput has 20% effect
}
if (cyclecounter>=(int (cyclepart*19))&&cyclecounter<=(int (cyclepart*19))+cyclepart)//the 20th cyclepart
{
rollcycle = rollinput; //rollinput has 100% effect
nickcycle = 0.1*nickinput; //nickinput has 10% effect
}
}
// half cycle done, last half starts here
if (cyclecounter >= (int (cyclepart*20)) && cyclecounter <= ((int (cyclepart*29))+cyclepart)) // if not skip 10 calculations
{ //Serial.print("C3 ");
if (cyclecounter>=(int (cyclepart*20))&&cyclecounter<=(int (cyclepart*20))+cyclepart)//the 21th cyclepart
{
rollcycle = rollinput; //rollinput has 100% effect
nickcycle = 0.1*nickinneg; //nickinput-negatif has 10% effect
}
if (cyclecounter>=(int (cyclepart*21))&&cyclecounter<=(int (cyclepart*21))+cyclepart)//the 22th cyclepart
{
rollcycle = 0.9*rollinput; //rollinput has 90% effect
nickcycle = 0.2*nickinneg; //nickinput-negatif has 20% effect
}
if (cyclecounter>=(int (cyclepart*22))&&cyclecounter<=(int (cyclepart*22))+cyclepart)//the 23th cyclepart
{
rollcycle = 0.8*rollinput; //rollinput has 80% effect
nickcycle = 0.3*nickinneg; //nickinput-negatif has 30% effect
}
if (cyclecounter>=(int (cyclepart*23))&&cyclecounter<=(int (cyclepart*23))+cyclepart)//the 24th cyclepart
{
rollcycle = 0.7*rollinput; //rollinput has 70% effect
nickcycle = 0.4*nickinneg; //nickinput-negatif has 40% effect
}
if (cyclecounter>=(int (cyclepart*24))&&cyclecounter<=(int (cyclepart*24))+cyclepart)//the 25th cyclepart
{
rollcycle = 0.6*rollinput; //rollinput has 60% effect
nickcycle = 0.5*nickinneg; //nickinput-negatif has 50% effect
}
if (cyclecounter>=(int (cyclepart*25))&&cyclecounter<=(int (cyclepart*25))+cyclepart)//the 26th cyclepart
{
rollcycle = 0.5*rollinput; //rollinput has 50% effect
nickcycle = 0.6*nickinneg; //nickinput-negatif has 60% effect
}
if (cyclecounter>=(int (cyclepart*26))&&cyclecounter<=(int (cyclepart*26))+cyclepart)//the 27th cyclepart
{
rollcycle = 0.4*rollinput; //rollinput has 40% effect
nickcycle = 0.7*nickinneg; //nickinput-negatif has 70% effect
}
if (cyclecounter>=(int (cyclepart*27))&&cyclecounter<=(int (cyclepart*27))+cyclepart)//the 28th cyclepart
{
rollcycle = 0.3*rollinput; //rollinput has 30% effect
nickcycle = 0.8*nickinneg; //nickinput-negatif has 80% effect
}
if (cyclecounter>=(int (cyclepart*28))&&cyclecounter<=(int (cyclepart*28))+cyclepart)//the 29th cyclepart
{
rollcycle = 0.2*rollinput; //rollinput has 20% effect
nickcycle = 0.9*nickinneg; //nickinput-negatif has 90% effect
}
if (cyclecounter>=(int (cyclepart*29))&&cyclecounter<=(int (cyclepart*29))+cyclepart)//the 30th cyclepart
{
rollcycle = 0.1*rollinput; //rollinput has 10% effect
nickcycle = nickinneg; //nickinput-negatif has 100% effect
}
}
//last kwart start here
if (cyclecounter >= (int (cyclepart*30)) && cyclecounter <= ((int (cyclepart*39))+cyclepart)) // if not skip the next 10 calculations
{ //Serial.print("C4 ");
if (cyclecounter>=(int (cyclepart*30))&&cyclecounter<=(int (cyclepart*30))+cyclepart)//the 31th cyclepart
{
rollcycle = 0.1*rollinneg; //rollinput-negatif has 10% effect
nickcycle = nickinneg; //nickinput-negatif has 100% effect
}
if (cyclecounter>=(int (cyclepart*31))&&cyclecounter<=(int (cyclepart*31))+cyclepart)//the 32th cyclepart
{
rollcycle = 0.2*rollinneg; //rollinput-negatif has 20% effect
nickcycle = 0.9*nickinneg; //nickinput-negatif has 90% effect
}
if (cyclecounter>=(int (cyclepart*32))&&cyclecounter<=(int (cyclepart*32))+cyclepart)//the 33th cyclepart
{
rollcycle = 0.3*rollinneg; //rollinput-negatif has 30% effect
nickcycle = 0.8*nickinneg; //nickinput-negatif has 80% effect
}
if (cyclecounter>=(int (cyclepart*33))&&cyclecounter<=(int (cyclepart*33))+cyclepart)//the 34th cyclepart
{
rollcycle = 0.4*rollinneg; //rollinput-negatif has 40% effect
nickcycle = 0.7*nickinneg; //nickinput-negatif has 70% effect
}
if (cyclecounter>=(int (cyclepart*34))&&cyclecounter<=(int (cyclepart*34))+cyclepart)//the 35th cyclepart
{
rollcycle = 0.5*rollinneg; //rollinput-negatif has 50% effect
nickcycle = 0.6*nickinneg; //nickinput-negatif has 60% effect
}
if (cyclecounter>=(int (cyclepart*35))&&cyclecounter<=(int (cyclepart*35))+cyclepart)//the 36th cyclepart
{
rollcycle = 0.6*rollinneg; //rollinput-negatif has 60% effect
nickcycle = 0.5*nickinneg; //nickinput-negatif has 50% effect
}
if (cyclecounter>=(int (cyclepart*36))&&cyclecounter<=(int (cyclepart*36))+cyclepart)//the 37th cyclepart
{
rollcycle = 0.7*rollinneg; //rollinput-negatif has 70% effect
nickcycle = 0.4*nickinneg; //nickinput-negatif has 40% effect
}
if (cyclecounter>=(int (cyclepart*37))&&cyclecounter<=(int (cyclepart*37))+cyclepart)//the 38th cyclepart
{
rollcycle = 0.8*rollinneg; //rollinput-negatif has 80% effect
nickcycle = 0.3*nickinneg; //nickinput-negatif has 30% effect
}
if (cyclecounter>=(int (cyclepart*38))&&cyclecounter<=(int (cyclepart*38))+cyclepart)//the 39th cyclepart
{
rollcycle = 0.9*rollinneg; //rollinput-negatif has 90% effect
nickcycle = 0.2*nickinneg; //nickinput-negatif has 20% effect
}
if (cyclecounter>=(int (cyclepart*39))+cyclepart)//the last cycle part
{
rollcycle = rollinneg; //rollinput-negatif has 100% effect
nickcycle = 0.1*nickinneg; //nickinput-negatif has 10% effect
}
}
motornick = map(nickcycle, -500, 500, 100, 0);
motorroll = map(rollcycle, -500, 500, 100, 0);
/*
Serial.print(int (cyclecounter)); Serial.print(",");
Serial.print(int (cyclepart*1)); Serial.print(",");
Serial.print(int (cyclepart*2)); Serial.print(",");
Serial.print(int (cyclepart*3)); Serial.print(",");
Serial.print(int (cyclepart*4)); Serial.print(",");
Serial.print(int (cyclepart*5)); Serial.print(",");
Serial.print(int (cyclepart*6)); Serial.print(",");
Serial.print(int (cyclepart*7)); Serial.print(",");
Serial.print(int (cyclepart*8)); Serial.print(",");
Serial.print(int (cyclepart*9)); Serial.print(",");
Serial.print(int (cyclepart*10)); Serial.print(",");
Serial.print(int (cyclepart*11)); Serial.print(",");
Serial.print(int (cyclepart*12)); Serial.print(",");
Serial.print(int (cyclepart*13)); Serial.print(",");
Serial.print(int (cyclepart*14)); Serial.print(",");
Serial.print(int (cyclepart*15)); Serial.print(",");
Serial.print(int (cyclepart*16)); Serial.print(",");
Serial.print(int (cyclepart*17)); Serial.print(",");
Serial.print(int (cyclepart*18)); Serial.print(",");
Serial.print(int (cyclepart*19)); Serial.print(",");
Serial.print(int (cyclepart*20)); Serial.print(",");
Serial.print(int (cyclepart*21)); Serial.print(",");
Serial.print(int (cyclepart*22)); Serial.print(",");
Serial.print(int (cyclepart*23)); Serial.print(",");
Serial.print(int (cyclepart*24)); Serial.print(",");
Serial.print(int (cyclepart*25)); Serial.print(",");
Serial.print(int (cyclepart*26)); Serial.print(",");
Serial.print(int (cyclepart*27)); Serial.print(",");
Serial.print(int (cyclepart*28)); Serial.print(",");
Serial.print(int (cyclepart*29)); Serial.print(",");
Serial.print(int (cyclepart*30)); Serial.print(",");
Serial.print(int (cyclepart*31)); Serial.print(",");
Serial.print(int (cyclepart*32)); Serial.print(",");
Serial.print(int (cyclepart*33)); Serial.print(",");
Serial.print(int (cyclepart*34)); Serial.print(",");
Serial.print(int (cyclepart*35)); Serial.print(",");
Serial.print(int (cyclepart*36)); Serial.print(",");
Serial.print(int (cyclepart*37)); Serial.print(",");
Serial.print(int (cyclepart*38)); Serial.print(",");
Serial.print(int (cyclepart*39)); Serial.println(",");
Serial.println(cyclepart);*/
//Serial.print("Nick "); Serial.print(nickcycle); // nicksignaal naar servo
//Serial.print("");
//Serial.print("Roll "); Serial.println(rollcycle); // nicksignaal naar servo
//Serial.print(micros()-time0);Serial.println("na calc");
}
/* This function will initialise the module and only needs to be run once
after the module is first powered up or reset */
void configMag() {
uint8_t mag_name;
// make sure that the device is connected
Wire.beginTransmission(MAG_ADDRESS);
Wire.write((byte) 0x0A); // Identification Register A
Wire.endTransmission();
Wire.beginTransmission(MAG_ADDRESS);
Wire.requestFrom(MAG_ADDRESS, 1);
mag_name = Wire.read();
Wire.endTransmission();
if(mag_name != 0x48) {
Serial.println("HMC5883L not found!");
Serial.print(mag_name, HEX);
Serial.println(" found, should be 0x48");
delay(1000);
digitalWrite (error, HIGH);
}
else
{
digitalWrite (error, HIGH);
delay (200);
digitalWrite (initialyze, LOW);
digitalWrite (error, HIGH);
delay (200);
digitalWrite (initialyze, HIGH);
digitalWrite (error, LOW);
delay (200);
digitalWrite (initialyze, LOW);
digitalWrite (error, HIGH);
delay (200);
digitalWrite (error, LOW);
digitalWrite (initialyze, HIGH);
}
// Register 0x00: CONFIG_A
// normal measurement mode (0x00) and 75 Hz ODR (0x18)
Wire.beginTransmission(MAG_ADDRESS);
Wire.write((byte) 0x00);
Wire.write((byte) 0x18);
Wire.endTransmission();
delay(5);
// Register 0x01: CONFIG_B
// default range of +/- 130 uT (0x20)
Wire.beginTransmission(MAG_ADDRESS);
Wire.write((byte) 0x01);
Wire.write((byte) 0x20);
Wire.endTransmission();
delay(5);
// Register 0x02: MODE
// continuous measurement mode at configured ODR (0x00)
// possible to achieve 160 Hz by using single measurement mode (0x01) and DRDY
Wire.beginTransmission(MAG_ADDRESS);
Wire.write((byte) 0x02);
Wire.write((byte) 0x01);
Wire.endTransmission();
delay(200);
}
// read 6 bytes (x,y,z magnetic field measurements) from the magnetometer
void readMag() {
// multibyte burst read of data registers (from 0x03 to 0x08)
Wire.beginTransmission(MAG_ADDRESS);
Wire.write((byte) 0x03); // the address of the first data byte
Wire.endTransmission();
Wire.beginTransmission(MAG_ADDRESS);
Wire.requestFrom(MAG_ADDRESS, 6); // Request 6 bytes
int i = 0;
while(Wire.available() >0 && i < 6)
{
mag_buffer[i] = Wire.read(); // Read one byte
i++;
}
Wire.read();
Wire.endTransmission();
// combine the raw data into full integers (HMC588L sends MSB first)
// ________ MSB _______ _____ LSB ____
mag_raw[0] = (mag_buffer[0] << 8) | mag_buffer[1]; //x
mag_raw[1] = (mag_buffer[2] << 8) | mag_buffer[3]; //z
mag_raw[2] = (mag_buffer[4] << 8) | mag_buffer[5]; //y
// put the device back into single measurement mode
Wire.beginTransmission(MAG_ADDRESS);
Wire.write((byte) 0x02);
Wire.write((byte) 0x01);
Wire.endTransmission();
}
// simple interrupt service routine
void calcPITCH()
{
// if the pin is high, its a rising edge of the signal pulse, so lets record its value
if(digitalRead(PITCH_IN_PIN) == HIGH)
{
ulPITCHStart = micros();
}
else
{
// else it must be a falling edge, so lets get the time and subtract the time of the rising edge
// this gives use the time between the rising and falling edges i.e. the pulse duration.
unPITCHInShared = (uint16_t)(micros() - ulPITCHStart);
// use set the PITCH flag to indicate that a new PITCH signal has been received
bUpdateFlagsShared |= PITCH_FLAG;
}
}
void calcNICK()
{
if(digitalRead(NICK_IN_PIN) == HIGH)
{
ulNICKStart = micros();
}
else
{
unNICKInShared = (uint16_t)(micros() - ulNICKStart);
bUpdateFlagsShared |= NICK_FLAG;
}
}
void calcROLL()
{
if(digitalRead(ROLL_IN_PIN) == HIGH)
{
ulROLLStart = micros();
}
else
{
unROLLInShared = (uint16_t)(micros() - ulROLLStart);
bUpdateFlagsShared |= ROLL_FLAG;
}
}
void calcYAW()
{
if(digitalRead(YAW_IN_PIN) == HIGH)
{
ulYAWStart = micros();
}
else
{
unYAWInShared = (uint16_t)(micros() - ulYAWStart);
bUpdateFlagsShared |= YAW_FLAG;
}
}
void calcCALYAW()
{
if(digitalRead(CALYAW_IN_PIN) == HIGH)
{
ulCALYAWStart = micros();
}
else
{
unCALYAWInShared = (uint16_t)(micros() - ulCALYAWStart);
bUpdateFlagsShared |= CALYAW_FLAG;
}
}
void calcMOTOR()
{
if(digitalRead(MOTOR_IN_PIN) == HIGH)
{
ulMOTORStart = micros();
}
else
{
unMOTORInShared = (uint16_t)(micros() - ulMOTORStart);
bUpdateFlagsShared |= MOTOR_FLAG;
}
}