Diff: main.cpp

Revision:

8:03e50b5d9d10

Parent:

7:83d56dc36f01

--- a/main.cpp	Fri May 25 08:43:42 2018 +0000
+++ b/main.cpp	Sun Jun 17 11:59:39 2018 +0000
@@ -18,7 +18,7 @@
 //SD Card Object
 SDBlockDevice sd(PB_5, PB_4, PB_3, PA_4);
 //CAN
-CAN can(PD_0, PD_1);
+CAN can(PD_0, PD_1, 500000);
 //tRF
 Serial tRF(PC_12, PD_2, 19200);
 
@@ -32,10 +32,12 @@
 CANMessage msgLVDTFront;    // Left, Right, Steering Wheel 
 CANMessage msgLVDTRear;     // Left, Right
 CANMessage msgBrakes;       // Brake system preassure, Braking On/Off
+CANMessage msgKm;           // KM
 CANMessage msgGPS;          // GPS position data
 CANMessage msgGForce;       // Accel data
 CANMessage msgGyro;         // Gyro data
-char* temp_string;         // Temp data
+CANMessage msgTest;
+char* temp_string;          // Temp data
 int CANlen = 0;
 //Some stuff for CAN
 // Variables received from DTA, LVDTs and brakes
@@ -46,10 +48,10 @@
 int FL_LVDT_Ref,FR_LVDT_Ref,RL_LVDT_Ref,RR_LVDT_Ref;
 uint16_t rx_flag=0x0000;        // Receive specific CAN data message
 uint8_t lvdtref=0x0F;          // Flag if refferent LVDT value is received (first received LVDT value, 1=no, 0=yes). From highest to lowest bit: LL,LR,RL,RR.
-
+float kilometraza;
 /*-----------tRF Send Telemetry-----------*/
 void tRF_sendCAN(CANMessage SendMe) {
-    tRF.printf("%x%s%d%d%d", SendMe.id, SendMe.data, SendMe.len, SendMe.format,SendMe.type);
+    tRF.printf("S%x%x\n", SendMe.id, SendMe.data);
 }
 
 /*------------------MAIN------------------*/
@@ -59,28 +61,28 @@
     
 //10DOF INIT
     imu.reset();
-    pc.printf("RESET DONE!");
+   // pc.printf("RESET DONE!");
     while(!imu.check()){
-        pc.printf("ejbg");
+        //pc.printf("ejbg");
             }
-    pc.printf("CHECK DONE!");
+    //pc.printf("CHECK DONE!");
 //10DOF SET MODE AND CALIBRATE
     imu.setmode(OPERATION_MODE_IMUPLUS);
     imu.get_calib();
 // Display sensor information
-    pc.printf("BNO055 found\r\n\r\n");
+    /*pc.printf("BNO055 found\r\n\r\n");
     pc.printf("Chip          ID: %0z\r\n",imu.ID.id);
     pc.printf("Accelerometer ID: %0z\r\n",imu.ID.accel);
     pc.printf("Gyroscope     ID: %0z\r\n",imu.ID.gyro);
     pc.printf("Magnetometer  ID: %0z\r\n\r\n",imu.ID.mag);
     pc.printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]);
-    pc.printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);
+    pc.printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);*/
 // Display chip serial number
-    for (int i = 0; i<4; i++){
+    /*for (int i = 0; i<4; i++){
         pc.printf("%0z.%0z.%0z.%0z\r\n",imu.ID.serial[i*4],imu.ID.serial[i*4+1],imu.ID.serial[i*4+2],imu.ID.serial[i*4+3]);
-    }
+    }*/
     
-    pc.printf("\r\n");
+    //pc.printf("\r\n");
     
 //tRF INIT
   tRF.printf("+++");
@@ -95,15 +97,15 @@
   tRF.printf("S201=4\r");
   // Go to send mode
   tRF.printf("ATO\r");
-  pc.printf("tRF INIT END\n");
+ // pc.printf("tRF INIT END\n");
   
 //SD CARD
-    printf("Initialise\n");
+    //printf("Initialise\n");
     //FileSystemLike(*sd);
 
 // call the SDBlockDevice instance initialisation method. (not needed)
     if ( sd.init() != 0) {
-        printf("Init failed \n");
+        //printf("Init failed \n");
         errorCode(FATAL);
     }    
     
@@ -111,16 +113,20 @@
     FATFileSystem fs("sd", &sd);
     
 // Open to WRITE
-    printf("Write to a file\n");
-    FILE* fp = fopen("/sd/WriteMeBaby.txt","a");
-    
+    //printf("Write to a file\n");
+    FILE* fp = fopen("/sd/Test.txt","a");
+    //FILE* km = fopen("/sd/kilometraza.txt","r+");
     if (fp == NULL) {
         error("Could not open file for read\n");
         errorCode(FATAL);
     }   
-    
+    /*if (km == NULL) {
+        error("Could not open file for read\n");
+        errorCode(FATAL);
+    }  
+    fscanf(km, "%f", &kilometraza);*/
 //Write something to SD
-    fputs("Drumska Strela!\r\n", fp);
+    //fputs("Drumska Strela!\r\n", fp);
 /*
 //Close File
     fclose(fp);
@@ -132,24 +138,27 @@
 */
 /*----------------MAIN LOOP----------------*/
     while (true) {
+        //printf("In while\n");
 //GET YPR
         imu.get_angles();
-        pc.printf("Yaw: %5.1f Pitch: %5.1f Roll: %5.1f\r\n", imu.euler.yaw, imu.euler.pitch, imu.euler.roll);
+        //pc.printf("Yaw: %5.1f Pitch: %5.1f Roll: %5.1f\r\n", imu.euler.yaw, imu.euler.pitch, imu.euler.roll);
         fprintf(fp, "Yaw: %5.1f Pitch: %5.1f Roll: %5.1f\r\n", imu.euler.yaw, imu.euler.pitch, imu.euler.roll);
 //GET ACCEL
         imu.get_accel();
-        pc.printf("ACX: %5.1f ACY: %5.1f ACZ: %5.1f\r\n", imu.accel.x, imu.accel.y, imu.accel.z);
+        //pc.printf("ACX: %5.1f ACY: %5.1f ACZ: %5.1f\r\n", imu.accel.x, imu.accel.y, imu.accel.z);
         fprintf(fp, "ACX: %5.1f ACY: %5.1f ACZ: %5.1f\r\n", imu.accel.x, imu.accel.y, imu.accel.z);
 //GET GPS DATA
+        //printf("GPS\n");
         NMEA_TYPE = gps.sample();
         if (1) {
-            pc.printf("LON: %f, LAT: %f, SIV: %d, TYPE: %d\r\n",gps.get_nmea_longitude(), gps.get_dec_latitude(), gps.get_satelites(), NMEA_TYPE);
+            //pc.printf("LON: %f, LAT: %f, SIV: %d, TYPE: %d\r\n",gps.get_nmea_longitude(), gps.get_dec_latitude(), gps.get_satelites(), NMEA_TYPE);
             fprintf(fp, "LON: %f, LAT: %f, SIV: %d, TYPE: %d\r\n",gps.get_nmea_longitude(), gps.get_dec_latitude(), gps.get_satelites(), NMEA_TYPE);
         }
 //CAN TEST
+        //printf("CANUpdate\n");
         UpdateInfo();
         //Send telemetry data
-        tRF.printf("POYY KEJTERI\n");
+        //tRF.printf("POYY KEJTERI\n");
         //Store data
         fprintf(fp, "Rpm0: %d, Speed0: %d, Gear0: %d, Water_Temp0: %d, Oil_Temp0: %d, TPS0: %d, Brakes0: %d, Oil_P0: %d, MAP0: %d, Air_Temp0: %d, Lambda0: %d, Volts0: %d, Crank0%d\r\n",\
         Rpm0,Speed0,Gear0,Water_Temp0,Oil_Temp0,TPS0,Brakes0,Oil_P0,MAP0,Air_Temp0,Lambda0,Volts0,Crank0);
@@ -159,6 +168,7 @@
         FL_LVDT0,FR_LVDT0,RL_LVDT0,RR_LVDT0,FL_LVDT,FR_LVDT,RL_LVDT,RR_LVDT);
         
 //tRF SendAll
+        //printf("sendCAN\n");
        tRF_sendCAN(msgDTA1);
        tRF_sendCAN(msgDTA2);
        tRF_sendCAN(msgDTA3);
@@ -167,8 +177,15 @@
        tRF_sendCAN(msgDTA6);
        tRF_sendCAN(msgLVDTFront);
        tRF_sendCAN(msgLVDTRear);
-       tRF_sendCAN(msgBrakes);
-       sprintf(temp_string, "%.5fN%.5fE", gps.get_nmea_longitude(), gps.get_dec_latitude());
+       tRF_sendCAN(msgBrakes);/*
+       while(1) {
+       sprintf(temp_string, "401236789");
+       CANlen = strlen(temp_string);
+       tRF_sendCAN(CANMessage(0x2001, temp_string, CANlen));
+       wait(1);
+       tRF.printf("S20012D123678\n");
+       }*/
+       /*sprintf(temp_string, "%.5fN%.5fE", gps.get_nmea_longitude(), gps.get_dec_latitude());
        CANlen = strlen(temp_string);
        msgGPS = CANMessage(0x0009, temp_string, CANlen);
        tRF_sendCAN(msgGPS);
@@ -178,7 +195,7 @@
        tRF_sendCAN(msgGForce);
        sprintf(temp_string, "%.5fX%.5fY%.5fZ", imu.accel.x, imu.accel.y, imu.accel.z);
        CANlen = strlen(temp_string);
-       msgGyro= CANMessage(0x0011, temp_string, CANlen);
+       msgGyro= CANMessage(0x0011, temp_string, CANlen);*/
        
     }
 }
\ No newline at end of file