LV Steamfitters Updates

Author: rdaruwala

embedded/app/main/badgerloop_LV.cpp

@@ -12,7 +12,7 @@ extern "C"
     #include "lv_iox.h"
     #include "nav.h"
     #include "braking.h"
-    
+    extern void csvFormatShow();    
 #include "connStat.h"
     #include "proc_iox.h"
 	#include "imu.h"
@@ -29,7 +29,7 @@ int init() {
     initLVIox(true);
 
     /* Init all peripherals */
-/*	SetupIMU();  GOODBYE MY SWEET SUMMER CHILD */
+	SetupIMU();
     initRetros();
     initNav();
 
@@ -73,7 +73,7 @@ int main() {
             data->flags->brakeSecRetr = false;
         }
 
-        
+        csvFormatShow();
         if (data->state != 1 && (!checkTCPStatHV() || !checkTCPStat())) {
             printf("ERRS: %d\n", errs);
             errs += 1;

embedded/app/src/nav.c

@@ -58,13 +58,11 @@ void showNavData() {
 }
 
 void csvFormatHeader() {
-    printf("posx,posy,posz,velx,vely,velz,accelx,accely,accelz,pos,vel,accel\n");   
+    printf("accelx,accely,accelz,pos,vel,accel\n");   
 }
 
 void csvFormatShow() {
-    printf("%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f\n", 
-           getPosX(), getPosY(), getPosZ(), 
-           getDeltaVX(), getDeltaVY(), getDeltaVZ(), 
+    printf("%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f\n", 
            getAccelX(), getAccelY(), getAccelZ(),
            data->motion->pos, data->motion->vel, data->motion->accel);
 }

embedded/examples/solenoidTest.c

@@ -61,6 +61,8 @@ int main(int argc, char *argv[]) {
     int state = whatToDo(argc, argv);
     printf("STATE: %d\n", state);
     if (state < 0) return -1;
+    printf("start\n");
     solenoidSet(sol, state);
+    printf("end\n");
     return 0;
 }

embedded/peripherals/include/imu.h

@@ -54,6 +54,10 @@ float getDeltaVX();
 float getDeltaVY();
 float getDeltaVZ();
 
+float getVelX();
+float getVelY();
+float getVelZ();
+
 void getAccelData(float *fData);
 float getAccelX();
 float getAccelY();

embedded/peripherals/src/braking.c

@@ -97,7 +97,7 @@ static double avgDouble(double *arr, int size) {
 
 int brake() {
     brakePrimaryActuate();
-    usleep(500000);
+    usleep(5000000);
     if (limSwitchGet(PRIM_LIM_SWITCH)) {
         brakeSecondaryActuate();
     }

embedded/peripherals/src/imu.c

@@ -236,6 +236,27 @@ float getDeltaVZ(){
 	return ans;
 }
 
+float getVelX(){
+    sem_wait(&data->mutex);
+    float ans = data->velX;
+    sem_post(&data->mutex);
+    return ans;
+}
+
+float getVelY(){
+    sem_wait(&data->mutex);
+    float ans = data->velY;
+    sem_post(&data->mutex);
+    return ans;
+}
+
+float getVelZ(){
+    sem_wait(&data->mutex);
+    float ans = data->velZ;
+    sem_post(&data->mutex);
+    return ans;
+}
+
 float getAccelX(){
 	sem_wait(&data->mutex);
 	float ans = data->accelX;

middleware/src/LVTCPSocket.cpp

@@ -93,7 +93,7 @@ void *LVTCPLoop(void *arg){
 		}
 		read(new_socket, buffer, 1024); 
 
-		printf("RECEIVED: %s\n",buffer);  
+	//	printf("RECEIVED: %s\n",buffer);  
 
 		// Do things
 		if(!strncmp(buffer, "power off", MAX_COMMAND_SIZE)){

middleware/src/LVTelemetry_Loop.cpp

@@ -16,6 +16,7 @@
 extern "C" 
 {
     #include "lv_iox.h"
+    #include "imu.h"
 }
 
 
@@ -77,21 +78,28 @@ void *LVTelemetryLoop(void *arg)
 
 			// POSITION
 			Value pos;
-			pos.SetFloat(data->motion->pos);
-			
+			//pos.SetFloat(getPosX());
+            pos.SetFloat(0);
+		
+            
+           // printf("%f %f %f\n", getAccelX(), getAccelY(), getAccelZ());
+
+
 			// RETRO
 			Value retro;
 			retro.SetInt(data->motion->retroCount);
 
 			// VELOCITY - Change "X" to "Y" if need be
 			Value vel;
-			vel.SetFloat(data->motion->vel);
+			//vel.SetFloat(getVelX());
+            vel.SetFloat(0);
 
 			Value lstRet;
             lstRet.SetUint64(data->timers->lastRetro);
 			// ACCELERATION - Change "X" to "Y" if need be
 			Value accel;
-			accel.SetFloat(data->motion->accel);
+			accel.SetFloat(-1 * getAccelX());
+            //accel.SetFloat(0);
 
 			// PRESSURE VESSEL PRESSURE
 			Value pressureV;