implemented gy85 with yaw control
This commit is contained in:
parent
a4e7c3e33d
commit
e73c0bc69c
|
|
@ -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
|
||||
*/
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue