implemented gy85 with yaw control

This commit is contained in:
interfisch 2019-03-17 18:57:32 +01:00
parent a4e7c3e33d
commit e73c0bc69c
1 changed files with 72 additions and 8 deletions

View File

@ -7,14 +7,25 @@
//to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately
//set boot0 back to 0 to run program on powerup
#define DEBUG
//#define DEBUG
#define PIN_LED PC13
#define SENDPERIOD 20 //ms
#define SENDPERIOD 20
#define IMUUPDATEPERIOD 10 //ms
long last_imuupdated=0;
#include <IMUGY85.h>
//https://github.com/fookingthg/GY85
//ITG3200 and ADXL345 from https://github.com/jrowberg/i2cdevlib/tree/master/Arduino
//https://github.com/mechasolution/Mecha_QMC5883L //because QMC5883 on GY85 instead of HMC5883, source: https://circuitdigest.com/microcontroller-projects/digital-compass-with-arduino-and-hmc5883l-magnetometer
//in qmc5883L library read values changed from uint16_t to int16_t
IMUGY85 imu;
double ax, ay, az, gx, gy, gz, roll, pitch, yaw, mx,my,mz,ma;
double setYaw=0;
//from left to right. pins at bottom. chips on top
@ -94,6 +105,9 @@ void setup() {
radio.startListening();
Serial.println("Initializing IMU");
imu.init();
Serial.println("Initialized");
@ -101,6 +115,11 @@ void setup() {
void loop() {
if (millis()-last_imuupdated>IMUUPDATEPERIOD){
updateIMU();
last_imuupdated=millis();
}
//NRF24
nrf_delay=millis()-last_nrfreceive; //update nrf delay
if ( radio.available() )
@ -116,18 +135,15 @@ void loop() {
motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
if (!motorenabled){ //disable motors?
armed=false;
Serial.println("!motorenebled. armed=false");
}
if (lastnrfdata.speed==NRFDATA_CENTER && lastnrfdata.speed==NRFDATA_CENTER){ //arm only when centered
armed=true; //arm at first received packet
Serial.println("centered. armed=true");
}
uint8_t calcchecksum=(uint8_t)((lastnrfdata.steer+3)*(lastnrfdata.speed+13));
if (lastnrfdata.checksum!=calcchecksum){ //checksum not ok?
armed=false;
Serial.println("Checksum fail. armed=false");
}else{ //checksum ok
last_nrfreceive=millis();
}
@ -149,19 +165,48 @@ void loop() {
//y positive = forward
//x positive = right
setYaw+=((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*10/127;
while (setYaw<0){
setYaw+=360;
}
while (setYaw>=360){
setYaw-=360;
}
/*
Serial.print("setYaw=");
Serial.print(setYaw);
Serial.print(" Yaw=");
Serial.println(yaw);*/
}
}
if (armed && nrf_delay>=MAX_NRFDELAY){ //too long since last sucessful nrf receive
armed=false;
Serial.println("too long since last nrf data. armed=false");
}
if (armed){
//out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
//out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
out_speed=(int16_t)( ((int16_t)(lastnrfdata.speed)-NRFDATA_CENTER)*1000/127 );
out_steer=(int16_t)( ((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*1000/127 );
out_speed=(int16_t)( ((int16_t)(lastnrfdata.speed)-NRFDATA_CENTER)*1000/127 ); //-1000 to 1000
//out_steer=(int16_t)( ((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*1000/127 );
//Test Mag
double yawdiff=(setYaw-180)-(yaw-180); //following angle difference works only for angles [-180,180]. yaw here is [0,360]
yawdiff += (yawdiff>180) ? -360 : (yawdiff<-180) ? 360 : 0;
//yawdiff/=2;
int yawdiffsign=1;
if (yawdiff<0){
yawdiffsign=-1;
}
yawdiff=yawdiff*yawdiff; //square
yawdiff=constrain(yawdiff/2,0,800);
yawdiff*=yawdiffsign; //redo sign
out_steer=(int16_t)( yawdiff );
/*
Serial.print("Out steer=");
Serial.println(out_steer);*/
}else{ //took too long since last nrf data
out_steer=0;
@ -199,3 +244,22 @@ void loop() {
}
}
void updateIMU()
{
//Update Imu and write to variables
imu.update();
imu.getAcceleration(&ax, &ay, &az);
imu.getGyro(&gx, &gy, &gz);
imu.getMag(&mx, &my, &mz,&ma); //calibration data such as bias is set in IMUGY85.h
roll = imu.getRoll();
pitch = imu.getPitch();
yaw = imu.getYaw();
/*Directions:
* Components on top.
* Roll: around Y axis (pointing to the right), left negative
* Pitch: around X axis (pointing forward), up positive
* Yaw: around Z axis, CCW positive, 0 to 360
*/
}