hi there
i am new to programing. i have prepared a code for my robot. i worrks completly i compiled it many time uploaded without any problem eddited many times but suddenly it genrated an error. SERIALL1 is not decleared in this scop.i am using hc12 rf module with arduino mega 2560. code error is here…….
the complete code is here.
#include <LiquidCrystal.h>
// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(12, 11, 9, 8, 7, 6);
#include <Servo.h>
#include <HCPCA9685.h>
#define I2CAdd 0x40
HCPCA9685 HCPCA9685(I2CAdd);
#define trigPin1 A5
#define echoPin1 A6
char val;// variable to receive data from the serial port
int light= 53;
int motor1Pin1 = 2; // pin 2 on L293D IC
int motor1Pin2 = 3; // pin 7 on L293D IC
int motor2Pin1 = 4; // pin 10 on L293D IC
int motor2Pin2 = 5; // pin 15 on L293D IC
int in1 = A1;
int in2 = A2;
int in3 = A3;
int in4 = A4;
int cont1 = 0;
int cont2 = 0;
int cont3 = 0;
int cont4 = 0;
// Array to store pins nos
int remotePins [] = {A1, A2, A3, A4 };
const int sensor=A0;
float tempc;
float vout;
int servo1 = 10; //10 to 420
int servo2 = 200; //50 to 420
int servo3 = 420;//420
int servo4 = 150;
/
int servo5 = 220; //220 to 350
int servo6 = 420;//10 to 420
int servo7 = 200;
int servo8 = 20;//10 to 420
int servo9 = 150;
int servo10 = 250;
long duration, distance,Sensor;
void setup()
{
for (int i = 0; i < 5; i ++)
{
pinMode (remotePins [i], INPUT);
digitalWrite(remotePins [i], HIGH);
}
HCPCA9685.Init(SERVO_MODE);
HCPCA9685.Sleep(false);
pinMode(sensor,INPUT);
pinMode(motor1Pin1, OUTPUT);
pinMode(motor1Pin2, OUTPUT);
pinMode(motor2Pin1, OUTPUT);
pinMode(motor2Pin2, OUTPUT);
pinMode(light, OUTPUT);
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
// Move all Servos at start to center position
HCPCA9685.Servo(0, servo1);
HCPCA9685.Servo(1, servo2);
HCPCA9685.Servo(2, servo2);
HCPCA9685.Servo(3, servo3);
HCPCA9685.Servo(4, servo4);
HCPCA9685.Servo(5, servo5);
HCPCA9685.Servo(6, servo6);
HCPCA9685.Servo(7, servo7);
HCPCA9685.Servo(8, servo7);
HCPCA9685.Servo(9, servo8);
HCPCA9685.Servo(10, servo9);
HCPCA9685.Servo(11, 250); //250 to 400
HCPCA9685.Servo(12, servo10);
Serial1.begin(9600);
Serial1.begin(9600);
delay(3000);
lcd.begin(16, 2);
// Print a message to the LCD.
lcd.print(«HIRA COLLEGE»);
lcd.print(«[[[SARGODHA]]]»);
delay (3000);
lcd.clear();
lcd.print(«HUMONOIDE ROBOT»);
lcd.print(«,.,RF BASED,.,»);
delay (2000);
lcd.clear();
lcd.print(«ELECTRONICS «);
lcd.setCursor(2, 1);
lcd.print(» DEPARTMENT «);
delay (2000);
lcd.clear();
lcd.print(«ELECTRONICS»);
lcd.setCursor(2, 1);
lcd.print(» 3rd YEAR «);
delay (2000);
lcd.clear();
lcd.print(» FINAL YEAR «);
lcd.setCursor(2, 1);
lcd.print(» PROJECT «);
delay (2000);
lcd.clear();
lcd.print(» [[[ GRUOP ]]] «);
lcd.setCursor(2, 1);
lcd.print(«…MEMBERS… «);
delay (2000);
lcd.clear();
lcd.print(«M.FAISAL G/L»);
lcd.setCursor(2, 1);
lcd.print(«ROLL NO. 51012»);
delay (4000);
lcd.clear();
lcd.print(«ALI MANSOOR»);
lcd.setCursor(2, 1);
lcd.print(«ROLL NO. 51014»);
delay (3000);
lcd.clear();
lcd.print(«EHTISHAM ZIA»);
lcd.setCursor(2, 1);
lcd.print(«ROLL NO. 51011»);
delay (3000);
lcd.clear();
lcd.print(«ZARNAB HASSAN»);
lcd.setCursor(2, 1);
lcd.print(«ROLL NO. 51001»);
delay (3000);
lcd.clear();
lcd.print(«SUPERVISOR ENGR.»);
lcd.setCursor(2, 1);
lcd.print(«SHOAIB AKHTAR»);
delay (3000);
lcd.clear();
lcd.print(» SESSION «);
lcd.setCursor(2, 1);
lcd.print(» 2015 — 2018 «);
delay (2000);
lcd.clear();
lcd.print(«FYP 2018»);
lcd.setCursor(2, 1);
lcd.print(«HIRA COLLEGE «);
delay (2000);
lcd.clear();
lcd.print(» RF Based Self»);
lcd.setCursor(2, 1);
lcd.print(«Defence Robot»);
delay (2000);
}
void loop()
{
lcd.setCursor(0, 0);
lcd.display();
delay(3000);
//if( Serial.available()>0) // if data is available to read
//{
//val = Serial.read(); // read it and store it in ‘val’
//}
if( Serial1.available()>0) // if data is available to read
{
val = Serial1.read(); // read it and store it in ‘val’
}
SonarSensor(trigPin1, echoPin1);
Sensor = distance;
//Serial.println(Sensor);
if( val == ‘1’ )
{
if(Sensor>60)
{
digitalWrite(motor1Pin1, LOW);digitalWrite(motor1Pin2, HIGH); digitalWrite(motor2Pin1, LOW);digitalWrite(motor2Pin2, HIGH);
}
if(Sensor<60)
{
digitalWrite(motor1Pin1, LOW);digitalWrite(motor1Pin2, LOW); digitalWrite(motor2Pin1, LOW);digitalWrite(motor2Pin2, LOW);
}
}
if( val == ‘2’ )
{
if(Sensor>60)
{
digitalWrite(motor1Pin1, HIGH);digitalWrite(motor1Pin2, LOW); digitalWrite(motor2Pin1, HIGH);digitalWrite(motor2Pin2, LOW);
}
if(Sensor<60)
{
digitalWrite(motor1Pin1, LOW);digitalWrite(motor1Pin2, LOW); digitalWrite(motor2Pin1, LOW);digitalWrite(motor2Pin2, LOW);
}
}
if( val == ‘4’ )
{
digitalWrite(motor1Pin1, HIGH);digitalWrite(motor1Pin2, LOW); digitalWrite(motor2Pin1, LOW);digitalWrite(motor2Pin2, HIGH);
}
if( val == ‘3’ )
{
if(Sensor>60)
{
if (digitalRead (in1) == LOW)
{
digitalWrite(motor1Pin1, LOW);digitalWrite(motor1Pin2, HIGH); digitalWrite(motor2Pin1, HIGH);digitalWrite(motor2Pin2, LOW);
}
if (digitalRead (in1) == HIGH)
{
digitalWrite(motor1Pin1, LOW);digitalWrite(motor1Pin2, LOW); digitalWrite(motor2Pin1, LOW);digitalWrite(motor2Pin2, LOW);
delay(500);
digitalWrite(motor1Pin1, HIGH);digitalWrite(motor1Pin2, LOW); digitalWrite(motor2Pin1, LOW);digitalWrite(motor2Pin2, HIGH);
delay(500);
digitalWrite(motor1Pin1, LOW);digitalWrite(motor1Pin2, LOW); digitalWrite(motor2Pin1, LOW);digitalWrite(motor2Pin2, LOW);
delay(500);
val = ‘0’;
}
}
if(Sensor<60)
{
digitalWrite(motor1Pin1, LOW);digitalWrite(motor1Pin2, LOW); digitalWrite(motor2Pin1, LOW);digitalWrite(motor2Pin2, LOW);
}
}
if( val == ‘5’ )
{
digitalWrite(motor1Pin1, LOW);digitalWrite(motor1Pin2, LOW); digitalWrite(motor2Pin1, LOW);digitalWrite(motor2Pin2, LOW);
}
if( val == ‘S’ )
{
servo1 = 10;
servo2 = 200;
servo3 = 420;
servo4 = 250;
servo5 = 220;
servo6 = 420;
servo7 = 220;
servo8 = 20;
servo9 = 250;
servo10 = 250;
HCPCA9685.Servo(0, servo1);
HCPCA9685.Servo(1, servo2);
HCPCA9685.Servo(2, servo2);
HCPCA9685.Servo(3, servo3);
HCPCA9685.Servo(4, servo4);
HCPCA9685.Servo(5, servo5);
HCPCA9685.Servo(6, servo6);
HCPCA9685.Servo(7, servo7);
HCPCA9685.Servo(8, servo7);
HCPCA9685.Servo(9, servo8);
HCPCA9685.Servo(10, servo9);
HCPCA9685.Servo(11, 250); //250 to 400
HCPCA9685.Servo(12, servo10);
}
if( val == ‘K’ )
{if(servo2<420){servo2 = servo2+4;HCPCA9685.Servo(1, servo2);HCPCA9685.Servo(2, servo2);delay(20);}}
if( val == ‘k’ )
{if(servo2>50){servo2 = servo2-4;HCPCA9685.Servo(1, servo2);HCPCA9685.Servo(2, servo2);delay(20);}}
if( val == ‘C’ )
{if(servo7<420){servo7 = servo7+4;HCPCA9685.Servo(7, servo7);HCPCA9685.Servo(8, servo7);delay(20);}}
if( val == ‘c’ )
{if(servo7>50){servo7 = servo7-4;HCPCA9685.Servo(7, servo7);HCPCA9685.Servo(8, servo7);delay(20);}}
if( val == ‘B’ )
{if(servo6<420){servo6 = servo6+4;HCPCA9685.Servo(6, servo6);delay(20);}}
if( val == ‘b’ )
{if(servo6>80){servo6 = servo6-4;HCPCA9685.Servo(6, servo6);delay(20);}}
if( val == ‘I’ )
{if(servo3<420){servo3 = servo3+4;HCPCA9685.Servo(3, servo3);delay(20);}}
if( val == ‘i’ )
{if(servo3>10){servo3 = servo3-4;HCPCA9685.Servo(3, servo3);delay(20);}}
if( val == ‘N’ )
{if(servo4<420){servo4 = servo4+4;HCPCA9685.Servo(4, servo4);delay(20);}}
if( val == ‘n’ )
{if(servo4>10){servo4 = servo4-4;HCPCA9685.Servo(4, servo4);delay(20);}}
if( val == ‘D’ )
{if(servo8<420){servo8 = servo8+4;HCPCA9685.Servo(9, servo8);delay(20);}}
if( val == ‘d’ )
{if(servo8>10){servo8 = servo8-4;HCPCA9685.Servo(9, servo8);delay(20);}}
if( val == ‘M’ )
{if(servo9<420){servo9 = servo9+4;HCPCA9685.Servo(10, servo9);delay(20);}}
if( val == ‘m’ )
{if(servo9>10){servo9 = servo9-4;HCPCA9685.Servo(10, servo9);delay(20);}}
if( val == ‘G’ )
{if(servo1<350){servo1 = servo1+4;HCPCA9685.Servo(0, servo1);delay(20);}}
if( val == ‘g’ )
{if(servo1>10){servo1 = servo1-4;HCPCA9685.Servo(0, servo1);delay(20);}}
if( val == ‘h’ )
{if(servo5<350){servo5 = servo5+4;HCPCA9685.Servo(5, servo5);delay(20);}}
if( val == ‘H’ )
{if(servo5>220){servo5 = servo5-4;HCPCA9685.Servo(5, servo5);delay(20);}}
if( val == ‘J’ )
{if(servo10<420){servo10 = servo10+4;HCPCA9685.Servo(12, servo10);delay(20);}}
if( val == ‘j’ )
{if(servo10>10){servo10 = servo10-4;HCPCA9685.Servo(12, servo10);delay(20);}}
if( val == ‘F’ )
{HCPCA9685.Servo(11, 410); delay(1000); HCPCA9685.Servo(11, 250); val = ‘0’;}
if( val == ‘Z’ )
{lightON();}
if( val == ‘z’ )
{lightOFF();}
vout=analogRead(sensor);
vout=(vout*500)/1023;
tempc=vout;
if (digitalRead (in4) == HIGH)
{
if(cont1==0)
{
Serial1.println(«A»);
cont1 = 1;
delay(100);
}
}
else{
if(cont1==1)
{
Serial1.println(«a»);
cont1 = 0;
delay(100);
}
}
if (digitalRead (in3) == LOW)
{
if(cont2==0)
{
Serial1.println(«B»);
cont2 = 1;
delay(100);
}
}
else{
if(cont2==1)
{
Serial1.println(«b»);
cont2 = 0;
delay(100);
}
}
if (tempc>50)
{
if(cont3==0)
{
Serial1.println(«C»);
cont3 = 1;
delay(100);
}
}
else{
if(cont3==1)
{
Serial1.println(«c»);
cont3 = 0;
delay(100);
}
}
if (digitalRead (in2) == LOW)
{
if(cont4==0)
{
Serial1.println(«D»);
cont4 = 1;
delay(100);
}
}
else{
if(cont4==1)
{
Serial1.println(«d»);
cont4 = 0;
delay(100);
}
}
//Serial.print(«in DegreeC=»);
//Serial.print(«t»);
//Serial.println(tempc);
}
void lightON(){digitalWrite(light, HIGH);}
void lightOFF(){digitalWrite(light, LOW);}
void SonarSensor(int trigPin,int echoPin)
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
}
— Mon Jul 04, 2016 12:33 pm
#50179
Hello,
I’m trying to figure out the whole udp send/receive thing from Wemos/esp2866/arduino.io to max msp.
I started out with the ntp client example, from github because and here starts the madness. If I use the regular file that’s in the examples in arduino then I get a compiling error on my Imac. When I use the one below from the github on my Imac it compiles and is ready to be uploaded in my WemosD1 Mini, but, my Imac has problems uploading the files on my wemos. Whereas my macbook pro does achieve this (with the same setting and such). But then my macbook doesn’t compile the code and gives me following error.
ERROR:
WiFiUdpNtpClient.ino: In function ‘void setup()’:
WiFiUdpNtpClient:41: error: ‘Serial’ was not declared in this scope
WiFiUdpNtpClient.ino: In function ‘void loop()’:
WiFiUdpNtpClient:81: error: ‘Serial’ was not declared in this scope
WiFiUdpNtpClient.ino: In function ‘void printWifiStatus()’:
WiFiUdpNtpClient:159: error: ‘Serial’ was not declared in this scope
CODE:
Code: Select all/*
Udp NTP Client
Get the time from a Network Time Protocol (NTP) time server
Demonstrates use of UDP sendPacket and ReceivePacket
For more on NTP time servers and the messages needed to communicate with them,
see http://en.wikipedia.org/wiki/Network_Time_Protocol
created 4 Sep 2010
by Michael Margolis
modified 9 Apr 2012
by Tom Igoe
updated for the ESP8266 12 Apr 2015
by Ivan Grokhotkov
This code is in the public domain.
*/
#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
char ssid[] = "*************"; // your network SSID (name)
char pass[] = "********"; // your network password
unsigned int localPort = 2390; // local port to listen for UDP packets
/* Don't hardwire the IP address or we won't get the benefits of the pool.
* Lookup the IP address for the host name instead */
//IPAddress timeServer(129, 6, 15, 28); // time.nist.gov NTP server
IPAddress timeServerIP; // time.nist.gov NTP server address
const char* ntpServerName = "time.nist.gov";
const int NTP_PACKET_SIZE = 48; // NTP time stamp is in the first 48 bytes of the message
byte packetBuffer[ NTP_PACKET_SIZE]; //buffer to hold incoming and outgoing packets
// A UDP instance to let us send and receive packets over UDP
WiFiUDP udp;
void setup()
{
Serial.begin(115200);
Serial.println();
Serial.println();
// We start by connecting to a WiFi network
Serial.print("Connecting to ");
Serial.println(ssid);
WiFi.begin(ssid, pass);
while (WiFi.status() != WL_CONNECTED) {
delay(500);
Serial.print(".");
}
Serial.println("");
Serial.println("WiFi connected");
Serial.println("IP address: ");
Serial.println(WiFi.localIP());
Serial.println("Starting UDP");
udp.begin(localPort);
Serial.print("Local port: ");
Serial.println(udp.localPort());
}
void loop()
{
//get a random server from the pool
WiFi.hostByName(ntpServerName, timeServerIP);
sendNTPpacket(timeServerIP); // send an NTP packet to a time server
// wait to see if a reply is available
delay(1000);
int cb = udp.parsePacket();
if (!cb) {
Serial.println("no packet yet");
}
else {
Serial.print("packet received, length=");
Serial.println(cb);
// We've received a packet, read the data from it
udp.read(packetBuffer, NTP_PACKET_SIZE); // read the packet into the buffer
//the timestamp starts at byte 40 of the received packet and is four bytes,
// or two words, long. First, esxtract the two words:
unsigned long highWord = word(packetBuffer[40], packetBuffer[41]);
unsigned long lowWord = word(packetBuffer[42], packetBuffer[43]);
// combine the four bytes (two words) into a long integer
// this is NTP time (seconds since Jan 1 1900):
unsigned long secsSince1900 = highWord << 16 | lowWord;
Serial.print("Seconds since Jan 1 1900 = " );
Serial.println(secsSince1900);
// now convert NTP time into everyday time:
Serial.print("Unix time = ");
// Unix time starts on Jan 1 1970. In seconds, that's 2208988800:
const unsigned long seventyYears = 2208988800UL;
// subtract seventy years:
unsigned long epoch = secsSince1900 - seventyYears;
// print Unix time:
Serial.println(epoch);
// print the hour, minute and second:
Serial.print("The UTC time is "); // UTC is the time at Greenwich Meridian (GMT)
Serial.print((epoch % 86400L) / 3600); // print the hour (86400 equals secs per day)
Serial.print(':');
if ( ((epoch % 3600) / 60) < 10 ) {
// In the first 10 minutes of each hour, we'll want a leading '0'
Serial.print('0');
}
Serial.print((epoch % 3600) / 60); // print the minute (3600 equals secs per minute)
Serial.print(':');
if ( (epoch % 60) < 10 ) {
// In the first 10 seconds of each minute, we'll want a leading '0'
Serial.print('0');
}
Serial.println(epoch % 60); // print the second
}
// wait ten seconds before asking for the time again
delay(10000);
}
// send an NTP request to the time server at the given address
unsigned long sendNTPpacket(IPAddress& address)
{
Serial.println("sending NTP packet...");
// set all bytes in the buffer to 0
memset(packetBuffer, 0, NTP_PACKET_SIZE);
// Initialize values needed to form NTP request
// (see URL above for details on the packets)
packetBuffer[0] = 0b11100011; // LI, Version, Mode
packetBuffer[1] = 0; // Stratum, or type of clock
packetBuffer[2] = 6; // Polling Interval
packetBuffer[3] = 0xEC; // Peer Clock Precision
// 8 bytes of zero for Root Delay & Root Dispersion
packetBuffer[12] = 49;
packetBuffer[13] = 0x4E;
packetBuffer[14] = 49;
packetBuffer[15] = 52;
// all NTP fields have been given values, now
// you can send a packet requesting a timestamp:
udp.beginPacket(address, 123); //NTP requests are to port 123
udp.write(packetBuffer, NTP_PACKET_SIZE);
udp.endPacket();
}
SO, before I can even start figuring out the udp protocol and then how to send I2C data over it to max, I need this problem solved.
Need help!! 
I have Adafruit Ultimate GPS Breakout — 66 channel w/10 Hz updates — Version 3 with a Genuino Zero.
After update thw librarys i want to play the sketch on my Arduino. But i got the error message error: ‘Serial2’ was not declared in this scope.
I have Serial2 in line 35, in void setup() and in GPS_Init()
Befor the update the sketch and GPS run.
Maybe someone can help.
Thanks very much.
Code: Select all
#include <Arduino.h> //base driver
#include <Adafruit_GFX.h> // Core graphics library
#include <Adafruit_ILI9341.h>
#include <Adafruit_FT6206.h> // Hardware-specific library
#include <SPI.h>
#include <SD.h>
#include <Wire.h> //I2C driver
#include <Kalman.h>
#include <math.h>
#include <variant.h>
#include <Adafruit_GPS.h>
//#include <SoftwareSerial.h>
// Default values for Adafruit shield v2.
#define TFT_RST 8
#define TFT_DC 9
#define TFT_CS 10
#define SD_CS 4
// Instantiate the Serial2 class
// The FT6206 uses hardware I2C (SCL/SDA)
Adafruit_FT6206 ctp = Adafruit_FT6206();
//---HardSPI-----------Created the second SPI Pin on Pin 11 12 13
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);
//#define gpsSerial Serial1
//HardwareSerial gpsSerial(Serial1);
Adafruit_GPS GPS(&Serial2);
//Adafruit_GPS GPS(&Serial2);
//used for timer setup
#define CPU_HZ 48000000
#define TIMER_PRESCALER_DIV 1024
#define BLACK 0x0000
#define BLUE 0x001F
#define RED 0xF800
#define GREEN 0x07E0
#define CYAN 0x07FF
#define MAGENTA 0xF81F
#define YELLOW 0xFFE0
#define WHITE 0xFFFF
typedef struct {
int x;
int y;
int w;
int h;
}rect;
// Rotations 0,2 = portrait : 0->USB=right,upper : 2->USB=left,lower
// Rotations 1,3 = landscape : 1->USB=left,upper : 3->USB=right,lower
#define _rotation 2 //(0->3)
#define _numBtns 5
Adafruit_GFX_Button btn[_numBtns];
rect r[] = {10,200,65,50,
160,200,65,50,
85,140,65,50,
85,260,65,50,
85,200,65,50};
int x, y;
TS_Point p;
char *titleStr[_numBtns] = {"Left","Right","Up","Down","PRESS"};
#define SPEED_UNIT 1
#define BLUETOOTHCON 7
int BluetoothState = 0;
uint8_t system_counter=0;
uint32_t timetest,timetest1,timetestold; //for running speed test
char UART_FIFO_out(void);
void UART_FIFO_in(void);
void Send_LabVIEW_data(void);
void SD_record(void);
void SD_record_once(void);
void SD_file(void);
void I2C_Scaner(void);
void Send_GPS_data(void);
void TFT_Display(void);
void uart_update_kalman_val(void);
void startTimer3(int frequencyHz);
void setTimerFrequency3(int frequencyHz);
void TC3_Handler(void);
void startTimer4(int frequencyHz);
void setTimerFrequency4(int frequencyHz);
void TC4_Handler(void);
void GPS_Init(void);
void MPU6050_Init(void);
void TFT_init(void);
void Get_MPU6050_Data(void);
void KalmanFilter(void);
void Update_UART_Buff(void);
//--------------------------used for MPU6050-------------------------------------------
//#define RESTRICT_PITCH // Comment out to restrict roll to 90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
/* IMU Data */
double accX=0.1, accY=0.1, accZ=0.1;
double gyroX=0.1, gyroY=0.1, gyroZ=0.1;
int16_t tempRaw=0.1;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
double dt;
double roll=0.1,pitch=0.1;
double gyroXrate,gyroYrate;
uint32_t timer; // dt
uint8_t i2cData[14]; // Buffer for I2C data
uint8_t i2cWrite(uint8_t, uint8_t, bool);
uint8_t i2cWrite(uint8_t, uint8_t , uint8_t, bool);
uint8_t i2cRead(uint8_t , uint8_t *, uint8_t );
char SDfilename[15];
uint8_t recordflag=0;
uint16_t record_counter=0;
uint8_t SDpresentflag=0;
uint8_t SDfileflag=1;
uint8_t test1flag=1;
uint8_t TFT_display_flag=1;
//uart buff for Bluetooth FIFO
#define UART_BUFF_MAX 120
char UART_buff[UART_BUFF_MAX];
uint8_t uart_buff_head=0,uart_buff_end=0,uart_buff_lenth=0;
double uart_roll=1,uart_pitch=1,uart_yaw=1;
double uart_accX=1,uart_accY=1,uart_accZ=1;
double uart_gyroX=1,uart_gyroY=1,uart_gyroZ=1;
double uart_tempRaw=1,uart_gyroXangle=1,uart_gyroYangle=1,uart_gyroZangle=1;
double uart_accXangle=1,uart_accYangle=1,uart_accZangle=1;
uint8_t uart_count=0;
uint8_t uarttimertest=0;
File dataFile;
uint8_t Data_Mark=0;
uint8_t cable_data_send_mark=0;
uint32_t timerGPS=0;
void setup() {
uart_buff_head=0;
uart_buff_end=0;
uart_buff_lenth=0;
tft.begin();
if (! ctp.begin(40)) { // pass in 'sensitivity' coefficient
Serial.println("Couldn't start FT6206 touchscreen controller");
while (1);
}
tft.setRotation(_rotation);
tft.fillScreen(BLACK);
Serial.begin(115200); // For Cable connection
gpsSerial.begin(9600);
Serial1.begin(115200); // For Bluetooth (19200 is the max for receive, 115200 is max for send only)
//GPS.begin(9600);
//while (!Serial1);
Serial2.begin(9600); // For GPS
SerialUSB.begin(115200);
//while (!SerialUSB);
Serial.println("nAutomoticeIMU System Start");
Serial1.println("nAutomoticeIMU System Start");
SerialUSB.println("nAutomoticeIMU System Start");
//----------Pin I/O Init----------------
pinMode(BLUETOOTHCON, INPUT);
SD_Init();
Serial.println("SD");
GPS_Init();
Serial.println("GPS");
MPU6050_Init();
Serial.println("MPU");
TFT_init();
Serial.println("TFT");
//---------Timer Interrupt Init---------
timetest=0;
timetestold=0;
startTimer3(100); //50 //Unit: Hz
Serial.println("Timer3");
startTimer4(1600);//1000
cable_data_send_mark=0;
for (int j=0; j<_numBtns; j++){
btn[j].initButtonUL( &tft, r[j].x, r[j].y, r[j].w, r[j].h, WHITE, RED, WHITE, titleStr[j], 2 );
btn[j].drawButton(false);
}
}
unsigned long TimeT = 0;
void loop() {
//char uart_temp=0;
if( millis()-TimeT >= 100){
unsigned long Time1 = millis();
Serial.println("Touch_Display");
Touch_Display();
Serial.println("Send_LabVIEW_data");
Send_LabVIEW_data();
Serial.println("4");
if(TFT_display_flag){
TFT_Display();
SD_record();
}
TimeT = millis();
long Time3 = TimeT - Time1 ;
Serial.println(Time3);
}
//if(uart_buff_lenth>0)
// {
//uart_temp=UART_FIFO_out();
//Serial.print(uart_temp);
//uart_update_kalman_val();
// }
if (GPS.newNMEAreceived()) {
// a tricky thing here is if we print the NMEA sentence, or data
// we end up not listening and catching other sentences!
// so be very wary if using OUTPUT_ALLDATA and trytng to print out data
//Serial.println(GPS.lastNMEA()); // this also sets the newNMEAreceived() flag to false
if (GPS.parse(GPS.lastNMEA())) // this also sets the newNMEAreceived() flag to false
{
// approximately every 2 seconds or so, print out the current stats
/*if (millis() - timerGPS > 2000)
{
timerGPS = millis(); // reset the timer
//Send_GPS_data();
}*/
}
}
//if (timerGPS > millis()) timerGPS = millis();
}
void btn0Action() {
Serial.println("Btn0Action called.");
Serial.println("==================");
if(TFT_display_flag){
TFT_display_flag=0;
tft.setCursor(40, 90);
tft.setTextColor(WHITE);
tft.setTextSize(1);
tft.fillRect(40, 90 ,30, 10, BLACK);
tft.fillRect(0, 0 ,128, 140, BLACK);
tft.setCursor(17, 18);
tft.setTextColor(RED);
tft.setTextSize(2);
tft.println("Auto IMU Recorder");
tft.setCursor(22, 63);
tft.setTextColor(WHITE);
tft.setTextSize(1);
tft.println("Version: V1.0");
}
}
void btn1Action() {
Serial.println("Btn1Action called.");
tft.setCursor(8, 15);
Serial.println("==================");
if(!TFT_display_flag){
TFT_display_flag=1;
tft.fillRect(0, 0 ,230, 100, BLACK);
tft.setTextColor(WHITE);
tft.setTextSize(1);
//Menu
tft.setCursor(38, 25);
tft.setTextSize(1);
tft.print("Roll:");
tft.setCursor(32, 35);
tft.setTextSize(1);
tft.print("Pitch:");
tft.setTextSize(1);
tft.setCursor(44, 45);
tft.setTextSize(1);
tft.print("Yaw:");
tft.setTextSize(1);
tft.setCursor(30, 58);
tft.setTextSize(1);
tft.print("GPS/Qua.:");
tft.setTextSize(1);
tft.setCursor(30, 68);
tft.setTextSize(1);
tft.print("L:");
tft.setTextSize(1);
tft.setCursor(30, 78);
tft.print("B:");
tft.setTextSize(1);
tft.setCursor(30, 88);
tft.print("Speed:");
tft.setTextSize(1);
tft.setCursor(36, 98);
tft.print("Alt.:");
tft.setTextSize(1);
tft.setCursor(30,108);
tft.print("Satellites:");
}
}
void btn2Action() {
Serial.println("Btn2Action called.");
Serial.println("==================");
if(cable_data_send_mark){
cable_data_send_mark=0;
}
else
{
cable_data_send_mark=1;
}
}
void btn3Action() {
Serial.println("Btn3Action called.");
Serial.println("==================");
Data_Mark=1;
}
void btn4Action() {
Serial.println("Btn4Action called.");
Serial.println("==================");
if(recordflag == 0)
{
tft.fillRoundRect(105, 215, 10, 30, 2, RED);
tft.fillRoundRect(125, 215, 10, 30, 2, RED);
//tft.fillTriangle(48, 120, 48, 150, 80, 135, ILI9341_BLUE);
//tft.fillCircle(64, 120, 20 , ILI9341_RED);
recordflag=1;
tft.fillRect(28, 115 ,90, 8, BLACK);
tft.setCursor(28, 115);
tft.setTextSize(1);
if(!SDpresentflag)
{
tft.setCursor(100, 200);
tft.setTextColor(GREEN);
tft.println("Starting...");
}
else {
tft.setCursor(100, 200);
tft.setTextColor(RED);
tft.println("Recording...");
}
}
else {
tft.fillTriangle(100, 250, 100, 210, 140, 230, BLUE);
recordflag=0;
SDfileflag=1;
tft.fillRect(28, 115 ,90, 8, BLACK);
}
}
void btnHandler(byte c){
Serial.print("Btn hit = "); Serial.println(c);
switch (c) {
case 0: btn0Action();
break;
case 1: btn1Action();
break;
case 2: btn2Action();
break;
case 3: btn3Action();
break;
case 4: btn4Action();
break;
}
}
void Touch_Display() {
if (!ctp.touched()) {
return;
}
p = ctp.getPoint();
switch (_rotation) {
case 0:
x = 240 - p.x;
y = 320 - p.y;
break;
case 1: // p.x, p.y reversed
x = 320 - p.y;
y = p.x;
break;
case 2:
x = p.x;
y = p.y;
break;
case 3: // p.x, p.y reversed
x = p.y;
y = 240 - p.x;
break;
}
while (ctp.touched()) {
for (byte z = 0; z < _numBtns; z++) {
if (btn[z].contains(x,y)) {
btn[z].drawButton(true); //invert
btnHandler(z);
delay(200);
btn[z].drawButton(false);
}
}
}
}
//timer interrupt
void TC3_Handler() {
TcCount16* TC = (TcCount16*) TC3;
// If this interrupt is due to the compare register matching the timer count
if (TC->INTFLAG.bit.MC0 == 1) {
TC->INTFLAG.bit.MC0 = 1;
///////////////////////////////////////////////////
//timetest = micros();
//Main System
switch(system_counter) //200Hz
{
case 0: //Get MPU6050_Data
{
Serial.println("Entered Get_MPU6050_Data ");
// Get_MPU6050_Data();
break;
}
case 1: //Kalman Filter
{
KalmanFilter();
break;
}
case 2:
if(test1flag == 1)
{
Update_UART_Buff();
test1flag=0;
record_counter++;
}
break;
default:break;
}
/*
//Temperature Data
double temperature = (double)tempRaw / 340.0 + 36.53;
Serial.print(temperature); Serial.print("t");
*/
//Interrupt time test.
//timetest = micros()-timetest;
//Serial1.println(timetest);
system_counter++;
if(system_counter>2)
system_counter=0;
///////////////////////////////////////////////////
}
}
void TC4_Handler(){
TcCount16* TC = (TcCount16*) TC4;
// If this interrupt is due to the compare register matching the timer count
if (TC->INTFLAG.bit.MC0 == 1) {
TC->INTFLAG.bit.MC0 = 1;
///////////////////////////////////////////////////////
GPS.read(); //2000Hz (dely.c)
UART_FIFO_in();
///////////////////////////////////////////////////////
}
}
void I2C_Scaner(void){
uint8_t error, address;
int nDevicescounter;
Serial.println("nSearching for I2C decives...");
nDevicescounter = 0; //count the number of devices found
for (address = 1; address < 127; address++)
{
Wire.beginTransmission(address);
error = Wire.endTransmission();
if (error == 0)
{
Serial.print("I2C device found at address 0x");
if (address < 16)
Serial.print("0");
Serial.print(address, HEX);
Serial.println(" .");
nDevicescounter++;
}
else if (error == 4)
{
Serial.print("Unknow error at address 0x");
if (address < 16)
Serial.print("0");
Serial.print(address, HEX);
Serial.println(" .");
}
}
if (nDevicescounter == 0)
Serial.println("No I2C devices foundn");
else
Serial.println("donen");
}
/************************************************************************/
/************************Timer Interrupt driver**************************/
/************************************************************************/
/////////////////////////////////timer3//////////////////////////////////
void setTimerFrequency3(int frequencyHz) {
int compareValue = (CPU_HZ / (TIMER_PRESCALER_DIV * frequencyHz)) - 1;
TcCount16* TC = (TcCount16*) TC3;
// Make sure the count is in a proportional position to where it was
// to prevent any jitter or disconnect when changing the compare value.
TC->COUNT.reg = map(TC->COUNT.reg, 0, TC->CC[0].reg, 0, compareValue);
TC->CC[0].reg = compareValue;
while (TC->STATUS.bit.SYNCBUSY == 1);
}
//This is a slightly modified version of the timer setup found at:
//https://github.com/maxbader/arduino_tools
void startTimer3(int frequencyHz) {
REG_GCLK_CLKCTRL = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID (GCM_TCC2_TC3)) ;
while ( GCLK->STATUS.bit.SYNCBUSY == 1 );
TcCount16* TC = (TcCount16*) TC3;
TC->CTRLA.reg &= ~TC_CTRLA_ENABLE;
// Use the 16-bit timer
TC->CTRLA.reg |= TC_CTRLA_MODE_COUNT16;
while (TC->STATUS.bit.SYNCBUSY == 1);
// Use match mode so that the timer counter resets when the count matches the compare register
TC->CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ;
while (TC->STATUS.bit.SYNCBUSY == 1);
// Set prescaler to 1024
TC->CTRLA.reg |= TC_CTRLA_PRESCALER_DIV1024;
while (TC->STATUS.bit.SYNCBUSY == 1);
setTimerFrequency3(frequencyHz);
// Enable the compare interrupt
TC->INTENSET.reg = 0;
TC->INTENSET.bit.MC0 = 1;
NVIC_EnableIRQ(TC3_IRQn);
TC->CTRLA.reg |= TC_CTRLA_ENABLE;
while (TC->STATUS.bit.SYNCBUSY == 1);
}
//////////////////////////////////timer4///////////////////////////////////////////
void setTimerFrequency4(int frequencyHz) {
int compareValue = (CPU_HZ / (TIMER_PRESCALER_DIV * frequencyHz)) - 1;
TcCount16* TC = (TcCount16*) TC4;
// Make sure the count is in a proportional position to where it was
// to prevent any jitter or disconnect when changing the compare value.
TC->COUNT.reg = map(TC->COUNT.reg, 0, TC->CC[0].reg, 0, compareValue);
TC->CC[0].reg = compareValue;
while (TC->STATUS.bit.SYNCBUSY == 1);
}
//This is a slightly modified version of the timer setup found at:
//https://github.com/maxbader/arduino_tools
void startTimer4(int frequencyHz) { ////////////////////?test
REG_GCLK_CLKCTRL = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID (GCM_TC4_TC5)) ; //GCLK_CLKCTRL_GEN_GCLK1 the second one
while ( GCLK->STATUS.bit.SYNCBUSY == 1 );
TcCount16* TC = (TcCount16*) TC4;
TC->CTRLA.reg &= ~TC_CTRLA_ENABLE;
// Use the 16-bit timer
TC->CTRLA.reg |= TC_CTRLA_MODE_COUNT16;
while (TC->STATUS.bit.SYNCBUSY == 1);
// Use match mode so that the timer counter resets when the count matches the compare register
TC->CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ;
while (TC->STATUS.bit.SYNCBUSY == 1);
// Set prescaler to 1024
TC->CTRLA.reg |= TC_CTRLA_PRESCALER_DIV1024;
while (TC->STATUS.bit.SYNCBUSY == 1);
setTimerFrequency4(frequencyHz);
// Enable the compare interrupt
TC->INTENSET.reg = 0;
TC->INTENSET.bit.MC0 = 1;
NVIC_EnableIRQ(TC4_IRQn);
TC->CTRLA.reg |= TC_CTRLA_ENABLE;
while (TC->STATUS.bit.SYNCBUSY == 1);
}
/************************************************************************/
/*************************I2C driver for TFT*****************************/
/************************************************************************/
uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
}
uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data, length);
uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
if (rcode) {
Serial.print(F("i2cWrite failed: "));
Serial.println(rcode);
}
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}
uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
uint32_t timeOutTimer;
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
if (rcode) {
Serial.print(F("i2cRead failed: "));
Serial.println(rcode);
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}
Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading´
for (uint8_t i = 0; i < nbytes; i++) {
if (Wire.available())
data[i] = Wire.read();
else {
timeOutTimer = micros();
while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
if (Wire.available())
data[i] = Wire.read();
else {
Serial.println(F("i2cRead timeout"));
return 5; // This error value is not already taken by endTransmission
}
}
}
return 0; // Success
}
/************************************************************************/
/*****************SETUP & LOOP & INTERRUPT SUBFUNCTION*******************/
/************************************************************************/
/**Init Setup**/
void SD_Init(void)
{
if (!SD.begin(SD_CS)) {
SDpresentflag=0;
Serial.println("SD Init failed!");
}else
{
SDpresentflag=1;
Serial.println("SD Init success!");
}
Data_Mark=0;
}
void GPS_Init(void)
{
//Serial.begin(115200);
Adafruit_GPS GPS(&Serial2);
// 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
GPS.begin(9600);
// uncomment this line to turn on RMC (recommended minimum) and GGA (fix data) including altitude
GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
// uncomment this line to turn on only the "minimum recommended" data
//GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
// For parsing data, we don't suggest using anything but either RMC only or RMC+GGA since
// the parser doesn't care about other sentences at this time
// Set the update rate
GPS.sendCommand(PMTK_SET_NMEA_UPDATE_5HZ); // 1 Hz update rate
// For the parsing code to work nicely and have time to sort thru the data, and
// print it out we don't suggest using anything higher than 1 Hz
// Request updates on antenna status, comment out to keep quiet
GPS.sendCommand(PGCMD_ANTENNA);
// the nice thing about this code is you can have a timer0 interrupt go off
// every 1 millisecond, and read data from the GPS for you. that makes the
// loop code a heck of a lot easier!
}
void MPU6050_Init(void)
{
int16_t temp;
//------------I2C Init------------------
Wire.begin();
I2C_Scaner();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to 250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to 2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Can not find MPU6050..."));
tft.setCursor(10, 80);
tft.setTextColor(RED);
tft.setTextSize(1);
tft.println("Can not find MPU6050...");
while (1); //---------------------------------------------------------------
}
delay(500); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 6));
temp = ((i2cData[0] << 8) | i2cData[1]);
accY = (double) temp;
temp = ((i2cData[2] << 8) | i2cData[3]);
accX = (double) temp;
temp = ((i2cData[4] << 8) | i2cData[5]);
accZ = (double) temp;
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -�� to �� (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
roll = atan2(accY, accZ) * RAD_TO_DEG;
pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
}
void TFT_init(void)
{
//-------TFT Display Init--------------
//tft.initR(INITR_BLACKTAB); // initialize a ILI9341S chip, black tab
tft.fillScreen(BLACK); //It costs 337us to fill the whole screen by softSPI HardwareSPI cost around 170us
//tft.setRotation(2); //Change the direction of display
//TFT Print Titlt
tft.setCursor(17, 18);
tft.setTextColor(RED);
tft.setTextSize(2);
tft.println("Auto IMU Recorder");
tft.fillRect(40, 50 , 42, 7, BLACK);
tft.setCursor(40, 50);
tft.setTextColor(WHITE);
tft.setTextSize(1);
tft.print("Version: V1.0");
tft.fillTriangle(100, 250, 100, 210, 140, 230, BLUE);
TFT_display_flag=0;
}
/**Main Loop**/
void UART_FIFO_in(void)
{
while(Serial1.available()>0)
{
if(uart_buff_lenth<UART_BUFF_MAX)
{
UART_buff[uart_buff_end]= Serial1.read();
uart_buff_end++;
uart_buff_lenth++;
if(uart_buff_end >= UART_BUFF_MAX)
uart_buff_end=0;
}else
Serial1.read();
}
}
char UART_FIFO_out(void)
{
if(uart_buff_lenth > 0)
{
uart_buff_head++;
uart_buff_lenth--;
if(uart_buff_head >= UART_BUFF_MAX)
{
uart_buff_head=0;
return UART_buff[(UART_BUFF_MAX-1)];
}
return UART_buff[(uart_buff_head-1)];
}else
return 0;
}
void uart_update_kalman_val(void)
{
char tempval;
float tempf;
if (tempval == 0x54)
{
tempval = UART_FIFO_out();
if(tempval == 0x58)
{
tempval= (uint8_t) UART_FIFO_out();
tempf=tempval*0.0001;
kalmanX.setQangle(tempf);
kalmanY.setQangle(tempf);
Serial.println(tempf,4);
tempval= (uint8_t) UART_FIFO_out();
tempf=tempval*0.0001;
kalmanX.setQbias(tempf);
kalmanY.setQbias(tempf);
Serial.println(tempf,4);
tempval= (uint8_t) UART_FIFO_out();
tempf=tempval*0.001;
kalmanX.setRmeasure(tempf);
kalmanY.setRmeasure(tempf);
Serial.println(tempf,4);
Serial.print("n");
}
}
}
void Send_LabVIEW_data(void)
{
//Blue tooth Data communication
BluetoothState=digitalRead(BLUETOOTHCON);
if((BluetoothState == HIGH) && (test1flag == 0))
{
switch(uart_count)
{
case 1:
Serial1.print("TC");
Serial1.print("OA");
Serial1.print(uart_accX);
Serial1.print(';');
Serial1.print(uart_accY);
Serial1.print(';');
Serial1.print(uart_accZ);
Serial1.print(';');
Serial1.print(uart_gyroX);
Serial1.print(';');
Serial1.print(uart_gyroY);
Serial1.print(';');
Serial1.print(uart_gyroZ);
Serial1.print(';');
Serial1.print('n');
break;
case 2:
Serial1.print("TC");
Serial1.print("OB");
Serial1.print(uart_accXangle);
Serial1.print(';');
Serial1.print(uart_accYangle);
Serial1.print(';');
Serial1.print(uart_accZangle);
Serial1.print(';');
Serial1.print(uart_gyroXangle);
Serial1.print(';');
Serial1.print(uart_gyroYangle);
Serial1.print(';');
Serial1.print(uart_gyroZangle);
Serial1.print(';');
Serial1.print('n');
break;
case 3:
Serial1.print("TC");
Serial1.print("OC");
Serial1.print(uart_roll);
Serial1.print(';');
Serial1.print(uart_pitch);
Serial1.print(';');
Serial1.print(uart_yaw);
Serial1.print(';');
Serial1.print('n');
break;
case 4:
//GPS Data
Serial1.print("TC");
Serial1.print("OG");
Serial1.print(GPS.hour, DEC); Serial1.print(';');
Serial1.print(GPS.minute, DEC); Serial1.print(';');
Serial1.print(GPS.seconds, DEC); Serial1.print(';');
Serial1.print(GPS.milliseconds);Serial1.print(';');
Serial1.print(GPS.day, DEC); Serial1.print(';');
Serial1.print(GPS.month, DEC); Serial1.print(';');
Serial1.print("20");Serial1.print(GPS.year, DEC);Serial1.print(';');
Serial1.print((int)GPS.fix);Serial1.print(';');
Serial1.print((int)GPS.fixquality);Serial1.print(';');
Serial1.print(GPS.latitude, 4); Serial1.print(GPS.lat);Serial1.print(';');
Serial1.print(GPS.longitude, 4); Serial1.print(GPS.lon);Serial1.print(';');
Serial1.print(GPS.speed);Serial1.print(';');
Serial1.print(GPS.angle);Serial1.print(';');
Serial1.print(GPS.altitude);Serial1.print(';');
Serial1.print((int)GPS.satellites);Serial1.println(';');
break;
default:break;
}
}
if((cable_data_send_mark == 1) && (test1flag == 0))
{
switch(uart_count)
{
case 1:
SerialUSB.print("TC");
SerialUSB.print("OA");
SerialUSB.print(uart_accX);
SerialUSB.print(';');
SerialUSB.print(uart_accY);
SerialUSB.print(';');
SerialUSB.print(uart_accZ);
SerialUSB.print(';');
SerialUSB.print(uart_gyroX);
SerialUSB.print(';');
SerialUSB.print(uart_gyroY);
SerialUSB.print(';');
SerialUSB.print(uart_gyroZ);
SerialUSB.print(';');
SerialUSB.print(';');
break;
case 2:
break;
case 3:
SerialUSB.print("TC");
SerialUSB.print("OC");
SerialUSB.print(uart_roll);
SerialUSB.print(';');
SerialUSB.print(uart_pitch);
SerialUSB.print(';');
SerialUSB.print(uart_yaw);
SerialUSB.print(';');
SerialUSB.print('n');
break;
case 4:
//GPS Data
SerialUSB.print("TC");
SerialUSB.print("OG");
SerialUSB.print(GPS.hour, DEC); SerialUSB.print(';');
SerialUSB.print(GPS.minute, DEC); SerialUSB.print(';');
SerialUSB.print(GPS.seconds, DEC); SerialUSB.print(';');
SerialUSB.print(GPS.milliseconds);SerialUSB.print(';');
SerialUSB.print(GPS.day, DEC); SerialUSB.print(';');
SerialUSB.print(GPS.month, DEC); SerialUSB.print(';');
SerialUSB.print("20");SerialUSB.print(GPS.year, DEC);SerialUSB.print(';');
SerialUSB.print((int)GPS.fix);SerialUSB.print(';');
SerialUSB.print((int)GPS.fixquality);SerialUSB.print(';');
SerialUSB.print(GPS.latitude, 4); SerialUSB.print(GPS.lat);SerialUSB.print(';');
SerialUSB.print(GPS.longitude, 4); SerialUSB.print(GPS.lon);SerialUSB.print(';');
SerialUSB.print(GPS.speed);SerialUSB.print(';');
SerialUSB.print(GPS.angle);SerialUSB.print(';');
SerialUSB.print(GPS.altitude);SerialUSB.print(';');
SerialUSB.print((int)GPS.satellites);SerialUSB.println(';');
break;
default:break;
}
}
if(uart_count>=4)
{
test1flag=1;
uart_count=0;
}
uart_count++;
}
void TFT_Display(void)
{
tft.setTextColor(WHITE);
tft.setTextSize(1);
//time
tft.fillRect(75, 2 ,48, 7, BLACK);
tft.setCursor(75, 2);
tft.print(GPS.hour, DEC); tft.print(':');
tft.print(GPS.minute, DEC); tft.print(':');
tft.print(GPS.seconds, DEC);
//date
tft.fillRect(5, 2 ,60, 7, BLACK);
tft.setCursor(5, 2);
tft.print(GPS.day, DEC); tft.print('/');
tft.print(GPS.month, DEC); tft.print('/');
tft.print("20");tft.print(GPS.year, DEC);
//Angle
tft.fillRect(75, 25 ,42, 7, BLACK);
tft.setCursor(75, 25);
tft.print(((kalAngleX+compAngleX)/2));
tft.fillRect(75, 35 ,42, 7, BLACK);
tft.setCursor(75, 35);
tft.print(((kalAngleY+compAngleY)/2));
/*
tft.fillRect(20, 40 , 48 , 7, ILI9341_BLACK);
tft.setCursor(20, 40);
tft.print(compAngleX);
tft.fillRect(70, 40 ,42, 7, ILI9341_BLACK);
tft.setCursor(70, 40);
tft.print(compAngleY);
*/
tft.fillRect(70, 45 ,48, 7, BLACK);
tft.setCursor(70, 45);
tft.print(GPS.angle);
//GPS
if(GPS.fix)
tft.setTextColor(GREEN);
else
tft.setTextColor(RED);
tft.fillRect(80, 58 ,18, 7, BLACK);
tft.setCursor(80, 58);
tft.print((int)GPS.fix,1);tft.print('/');tft.print((int)GPS.fixquality);
tft.fillRect(68, 68 ,60, 7, BLACK);
tft.setCursor(68, 68);
tft.print(GPS.latitude, 4); tft.print((char)GPS.lat);
tft.fillRect(68, 78 ,60, 7, BLACK);
tft.setCursor(68, 78);
tft.print(GPS.longitude, 4); tft.print((char)GPS.lon);
tft.fillRect(80, 88 ,30, 7, BLACK);
tft.setCursor(80, 88);
tft.print((GPS.speed*SPEED_UNIT));
tft.fillRect(80, 98 ,36, 7, BLACK);
tft.setCursor(80, 98);
tft.print(GPS.altitude);
tft.fillRect(100, 108 ,12, 7, BLACK);
tft.setCursor(100, 108);
tft.print((int)GPS.satellites);
}
void SD_file(void)
{
//check filename and create
if(SDpresentflag && SDfileflag)
{
strcpy(SDfilename, "LOG000.TXT");
for (uint8_t i = 0; i < 255; i++)
{
SDfilename[3] = '0' + i/100;
SDfilename[4] = '0' + (i/10)%10;
SDfilename[5] = '0' + i%10;
if (! SD.exists(SDfilename)) {
break;
}
}
//creat file
dataFile = SD.open(SDfilename, FILE_WRITE);
if(dataFile)
{
dataFile.print("FILENAME:");
dataFile.println(SDfilename);
//dataFile.println("MarkFlagtGPS Status(Fix,Quality)tSatellitestDatetTime(UTC+0)tRoll(Kalman)tPitch(Kalman)tRoll(Complementary)tPitch(Complementary)tYaw(Angle From GPS, Not accuracy)tLatitudetLongitudetSpeed(knots)tAltitudetAccXtAccYtAccZ");
dataFile.println("MarkFlag;GPSstatus(Fix,Quality);Satellites;Date;Time;Roll(Kalman);Pitch(Kalman);Roll(Comp1);Pitch(Comp1);Yaw(AngleGPS,not accuracy);Latitude;Longitude;Speed(Knots);Altitude;Accx;Accy;Accz");
dataFile.close();
Serial.print("create ");
Serial.print(SDfilename);
Serial.println(" success.");
SDfileflag=0;
}
}
}
void SD_record(void)
{
if(recordflag && (record_counter>0))
{
SD_file();
SD_record_once();
record_counter=0;
}
Data_Mark=0;
if((BluetoothState == LOW) && (cable_data_send_mark == 0) && (test1flag != 1) )
test1flag=1;
}
void SD_record_once(void)
{
if(SDpresentflag && (!SDfileflag))
{
// open the file. note that only one file can be open at a time,
// so you have to close this one before opening another.
//SDfilename
dataFile = SD.open(SDfilename, FILE_WRITE);
// if the file is available, write to it:
if (dataFile) {
//if(Data_Mark==1)
// {
//dataFile.print("x");
//}
if(Data_Mark==1) dataFile.print("x");
else dataFile.print("no");
dataFile.print(";");
//Fix/Quality
dataFile.print((int)GPS.fix);dataFile.print(',');dataFile.print((int)GPS.fixquality);
dataFile.print(";");
//Statellites
if (GPS.fix) dataFile.print((int)GPS.satellites); //Satellites
else dataFile.print('0');
dataFile.print(";");
//Date Day/Month/Year
dataFile.print(GPS.day, DEC); dataFile.print('/');dataFile.print(GPS.month, DEC); dataFile.print("/20");dataFile.print(GPS.year, DEC);
dataFile.print(";");
//Time(UTC+0)
dataFile.print(GPS.hour, DEC); dataFile.print(':');dataFile.print(GPS.minute, DEC); dataFile.print(':');dataFile.print(GPS.seconds, DEC); dataFile.print('.');dataFile.print(GPS.milliseconds);
dataFile.print(";");
//Roll(Kalman)
dataFile.print(kalAngleX);
dataFile.print(";");
//Pitch(Kalman)
dataFile.print(kalAngleY);
dataFile.print(";");
//Roll(Complementary)
dataFile.print(compAngleX);
dataFile.print(";");
//Pitch(Complementary)
dataFile.print(compAngleY);
dataFile.print(";");
if (GPS.fix)
{
//Yaw(Angle From GPS, Not accuracy)
dataFile.print(GPS.angle);
dataFile.print(";");
//LatitudetLongitudetSpeed(knots)tAltitude
dataFile.print(GPS.latitude, 4); dataFile.print(GPS.lat);
dataFile.print(";");
//Longitude
dataFile.print(GPS.longitude, 4); dataFile.print(GPS.lon);
dataFile.print(";");
//Speed(knots)
dataFile.print((GPS.speed*SPEED_UNIT));
dataFile.print(";");
//Altitude
//dataFile.println(GPS.altitude);// geänderte
dataFile.print(GPS.altitude);
dataFile.print(";");
}else{
//Yaw(Angle From GPS, Not accuracy)
dataFile.print('0');
dataFile.print(";");
//LatitudetLongitudetSpeed(knots)tAltitude
dataFile.print('0');
dataFile.print(";");
//Longitude
dataFile.print('0');
dataFile.print(";");
//Speed(knots)
dataFile.print('0');
dataFile.print(";");
//Altitude
dataFile.print('0');
dataFile.print(";");
}
//Acc
dataFile.print(uart_accX);
dataFile.print(";");
dataFile.print(uart_accY);
dataFile.print(";");
dataFile.println(uart_accZ);
//dataFile.print("n");
dataFile.close();
}
// if the file isn't open, pop up an error:
else {
Serial.println("error opening datalog.txt");
}
}else
{
if (!SD.begin(SD_CS)) {
SDpresentflag=0;
Serial.println("SD Init failed!");
//tft.fillRoundRect(39, 115, 50, 40, 8, ILI9341_WHITE);
tft.fillTriangle(100, 250, 100, 210, 140, 230, BLUE);
recordflag=0;
tft.fillRect(100, 200 ,90, 8, BLACK);
tft.setCursor(100, 200);
tft.setTextColor(RED);
tft.setTextSize(1);
tft.println("SD open failed!");
}/*else
{
SDpresentflag=1;
if(SDpresentflag)
{
strcpy(SDfilename, "LOG000.TXT");
for (uint8_t i = 0; i < 255; i++)
{
SDfilename[3] = '0' + i/100;
SDfilename[4] = '0' + (i/10)%10;
SDfilename[5] = '0' + i%10;
// create if does not exist, do not open existing, write, sync after write
if (! SD.exists(SDfilename)) {
break;
}
}
}
tft.fillRect(28, 80 ,90, 10, ILI9341_BLACK);
tft.setCursor(28, 80);
tft.setTextColor(ILI9341_RED);
tft.setTextSize(1);
tft.println("Recording...");
}*/
}
}
void Send_GPS_data(void) //By normal uart
{
Serial.print("nTime: ");
Serial.print(GPS.hour, DEC); Serial.print(':');
Serial.print(GPS.minute, DEC); Serial.print(':');
Serial.print(GPS.seconds, DEC); Serial.print('.');
Serial.println(GPS.milliseconds);
Serial.print("Date: ");
Serial.print(GPS.day, DEC); Serial.print('/');
Serial.print(GPS.month, DEC); Serial.print("/20");
Serial.println(GPS.year, DEC);
Serial.print("Fix: "); Serial.print((int)GPS.fix);
Serial.print(" quality: "); Serial.println((int)GPS.fixquality);
if (GPS.fix) {
Serial.print("Location: ");
Serial.print(GPS.latitude, 4); Serial.print(GPS.lat);
Serial.print(", ");
Serial.print(GPS.longitude, 4); Serial.println(GPS.lon);
Serial.print("Speed (knots): "); Serial.println(GPS.speed);
Serial.print("Angle: "); Serial.println(GPS.angle);
Serial.print("Altitude: "); Serial.println(GPS.altitude);
Serial.print("Satellites: "); Serial.println((int)GPS.satellites);
}
}
/**Timer Interrupt**/
void Get_MPU6050_Data(void)
{
int16_t temp;
// Update all the values
while (i2cRead(0x3B, i2cData, 14));
Serial.println("Updating...");
temp = ((i2cData[0] << 8) | i2cData[1]);
accY = (double) -temp;
temp = ((i2cData[2] << 8) | i2cData[3]);
accX = (double) -temp;
temp = ((i2cData[4] << 8) | i2cData[5]);
accZ = (double) temp;
tempRaw = (i2cData[6] << 8) | i2cData[7];
temp = (i2cData[8] << 8) | i2cData[9];
gyroY = (double) temp;
temp = (i2cData[10] << 8) | i2cData[11];
gyroX = (double) temp;
temp = (i2cData[12] << 8) | i2cData[13];
gyroZ = (double) temp;
dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
}
void KalmanFilter(void)
{
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -�� to �� (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
roll = atan2(accY, accZ) * RAD_TO_DEG;
pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
gyroXrate = gyroX / 131.0; // Convert to deg/s
gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;;
} else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate() * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
// Reset the gyro angle when it has drifted too much
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
}
void Update_UART_Buff(void)
{
uart_roll=kalAngleX;
//uart_roll=compAngleY;
uart_pitch=kalAngleY;
//uart_pitch=compAngleX;
uart_yaw=0;
uart_accX=accX*2/32768;
uart_accY=accY*2/32768;
uart_accZ=accZ*2/32768;
uart_gyroX=gyroX;
uart_gyroY=gyroY;
uart_gyroZ=gyroZ;
uart_tempRaw=tempRaw;
uart_gyroXangle=gyroXangle;
uart_gyroYangle=gyroYangle;
uart_gyroZangle=0;
uart_accXangle=pitch;
uart_accZangle=0;
}