Working on being ready to do nav

Author: Ezra Boley

Makefile

@@ -25,8 +25,8 @@ endif
 
 LV_USER   := "debian"
 HV_USER	  := "ezra"
-LV_IP  := "192.168.1.126"
-HV_IP  := "192.168.1.147"
+LV_IP  := "192.168.0.6"
+HV_IP  := "192.168.0.7"
 
 ifdef BB
 BEAGLE := ${BBCC}

embedded/app/include/states.h

@@ -7,39 +7,49 @@
 
 
 /* Pressure sensor acceptable limits (in PSI) */
-#define PS1_BOTTOM_LIMIT_PRE        	1000
-#define PS1_BOTTOM_LIMIT_CRAWLPOST  	0
-#define PS1_TOP_LIMIT_PRE          		3000
-#define PS1_TOP_LIMIT_CRAWLPOST     	20
-
-#define PS2_BOTTOM_LIMIT_PRE        	150
-#define PS2_BOTTOM_LIMIT_CRAWLPOST  	0
-#define PS2_TOP_LIMIT_PRE           	275
+#define PS1_BOTTOM_LIMIT_IDLE           -50
+#define PS1_BOTTOM_LIMIT_PRE        	500
+#define PS1_BOTTOM_LIMIT_CRAWLPOST  	-50
+#define PS1_TOP_LIMIT_PRE          		1300
+#define PS1_TOP_LIMIT_CRAWLPOST     	50
+#define PS1_TOP_LIMIT_IDLE           	1300
+
+#define PS2_BOTTOM_LIMIT_IDLE        	-15
+#define PS2_BOTTOM_LIMIT_PRE        	120
+#define PS2_BOTTOM_LIMIT_CRAWLPOST  	-15
+#define PS2_TOP_LIMIT_IDLE        	    150
+#define PS2_TOP_LIMIT_PRE           	300
 #define PS2_TOP_LIMIT_CRAWLPOST     	20
 
 
-#define PS3_BOTTOM_LIMIT_PRE        	0
+#define PS3_BOTTOM_LIMIT_IDLE        	-10
+#define PS3_BOTTOM_LIMIT_PRE        	-15
 #define PS3_BOTTOM_LIMIT_BRAKING		150
-#define PS3_BOTTOM_LIMIT_CRAWLPOST		0
-#define PS3_TOP_LIMIT_PRE           	20
-#define PS3_TOP_LIMIT_BRAKING			275
-#define PS3_TOP_LIMIT_CRAWLPOST			20
-
+#define PS3_BOTTOM_LIMIT_CRAWLPOST		-15
+#define PS3_TOP_LIMIT_IDLE        	    15
+#define PS3_TOP_LIMIT_PRE           	15
+#define PS3_TOP_LIMIT_BRAKING			300
+#define PS3_TOP_LIMIT_CRAWLPOST			15
 
+#define SEC_PS1_BOTTOM_LIMIT_IDLE       -50
 #define SEC_PS1_BOTTOM_LIMIT_PRE		1000
 #define SEC_PS1_BOTTOM_LIMIT_CRAWLPOST	0
 #define SEC_PS1_TOP_LIMIT_PRE			3000
 #define SEC_PS1_TOP_LIMIT_CRAWLPOST		20
+#define SEC_PS1_TOP_LIMIT_IDLE          50
 
 
+#define SEC_PS2_BOTTOM_LIMIT_IDLE       -15
 #define SEC_PS2_BOTTOM_LIMIT_PRE		150
 #define SEC_PS2_BOTTOM_LIMIT_CRAWLPOST	0
 #define SEC_PS2_TOP_LIMIT_PRE			275
 #define SEC_PS2_TOP_LIMIT_CRAWLPOST		20
+#define SEC_PS2_TOP_LIMIT_IDLE          15
 
-
+#define SEC_PS3_BOTTOM_LIMIT_IDLE       -15
 #define SEC_PS3_BOTTOM_LIMIT			0
 #define SEC_PS3_TOP_LIMIT				20
+#define SEC_PS3_TOP_LIMIT_IDLE          15
 
 #define PV_TOP_LIMIT                20
 #define PV_BOTTOM_LIMIT             10

embedded/app/include/transitions.h

@@ -9,4 +9,5 @@ int toBraking(void);
 
 int toCrawl(void);
 
+int toRunFault(void);
 #endif

embedded/app/main/badgerloop_HV.cpp

@@ -9,6 +9,7 @@
 #include "data_dump.h"
 #include "connStat.h"
 
+
 extern "C" 
 {
     #include <signal.h>
@@ -19,6 +20,7 @@ extern "C"
     #include "data.h"
     #include "can_devices.h"
     #include "state_machine.h"
+    #include "NCD9830DBR2G.h"
 }
 
 int init() {
@@ -30,7 +32,7 @@ int init() {
     initProcIox(true);
     initHVIox(true);
     initMotor();   
-    
+    initPressureSensors();
     /* Allocate needed memory for state machine and create graph */
 	buildStateMachine();
 
@@ -67,8 +69,17 @@ int main() {
             exit(0);
 	    }
 	    runStateMachine();
-        if (data->flags->shouldBrake)
+        if (data->flags->shouldBrake) {
             signalLV((char *)"brake");
+            data->flags->shouldBrake = false;
+        }
+        if (data->flags->brakeInit) {
+            signalLV((char *)"primBrakeOff");
+            signalLV((char *)"secBrakeOff");
+            data->flags->brakeInit = false;
+        }
+
+        
         usleep(10000);
 
 		// Control loop

embedded/app/main/badgerloop_LV.cpp

@@ -5,12 +5,15 @@
 #include "connStat.h"
 #include "LVTelemetry_Loop.h"
 #include "LVTCPSocket.h"
+
+
 /* ADD SENSOR INCLUDES HERE */
 extern "C"
 {
     #include "lv_iox.h"
     #include "nav.h"
     #include "braking.h"
+
     #include "proc_iox.h"
 	#include "imu.h"
     #include <data.h>
@@ -29,23 +32,44 @@ int init() {
     initPressureMonitor();
     initNav();
     /* Init telemetry services */
-	SetupLVTelemetry((char *) DASHBOARD_IP, LV_TELEM_PORT);
+	SetupLVTelemetry((char *) DASHBOARD_IP, DASHBOARD_PORT);
 	SetupLVTCPServer();
 
     return 0;
 }
 
 int main() {
 
-
 	if (init() == 1) {
 		printf("Error in initialization! Exiting...\r\n");
 		exit(1);
 	}
-	while(1) {
+    flags_t *f = data->flags;
+	if (f == NULL) printf("its null\n");
+    while(1) {
 		usleep(100000);
-        
+        if (f->shouldBrake) {
+            brake();
+            f->shouldBrake = false;
+        }
+        if (f->brakePrimAct) {
+            brakePrimaryActuate();
+            f->brakePrimAct = false;
+        }
+        if (f->brakePrimRetr) {
+            brakePrimaryUnactuate();
+            f->brakePrimRetr = false;
+        }
+        if (f->brakeSecAct) {
+            brakeSecondaryActuate();
+            f->brakeSecAct = false;
+        }
+        if (f->brakeSecRetr) {
+            brakeSecondaryUnactuate();
+            f->brakeSecRetr = false;
+        }
 		// Control loop
 	}
     return 0;
+
 }

embedded/app/src/motor.c

@@ -93,18 +93,19 @@ bool getMotorIsOn() {
 int startMotor() {
     static bool isMCULatched = false;
     if (!isMCULatched) {
-        setMCULatch(true);
+/*        setMCULatch(true);*/
 /*        usleep(1000000);  //FIXME Not sure what to make this, .5 s for now*/
 /*        setMCULatch(false);*/
 /*        isMCULatched = true;*/
     }
-    if (rmsEnHeartbeat() != 0) return 1;
-	if (rmsClrFaults() != 0) return 1;
-	if (rmsInvDis() != 0) return 1;
+    rmsEnHeartbeat();
+	rmsClrFaults();
+	rmsInvDis();
 /*	idleMotor();*/
     sleep(3); /* Not ideal, but unless we have a way to check status this stays */
-    rmsInvEnNoTorque();
 	setMotorIsOn(true);
+    usleep(100000);
+    rmsInvEnNoTorque();
     return 0;
 }
 
@@ -122,8 +123,6 @@ int stopMotor() {
     if (rmsDischarge() != 0) return 1;        
     if (rmsInvDis() != 0) return 1;
 
-    usleep(500000);
-    data->flags->shouldBrake = true;
     return 0;
 }
 

embedded/app/src/nav.c

@@ -9,8 +9,8 @@
 
 #define FEET_TO_METERS(x) ((x) * 0.3048)
 #define USEC_TO_SEC(x)    ((x) / 1000000)
-#define STRIP_DISTANCE FEET_TO_METERS(100) /*m*/
-#define TOTAL_DISTANCE FEET_TO_METERS(1000) /*m, not real TODO change*/
+#define STRIP_DISTANCE FEET_TO_METERS(10) /*m*/
+#define TOTAL_DISTANCE FEET_TO_METERS(100) /*m, not real TODO change*/
 #define TOTAL_RUN_TIME 30   /* s */
 #define EXPECTED_DECEL 9.8  /* m/s/s */
 
@@ -148,26 +148,38 @@ void filterMotion(int filterType) {
     data->motion->pos = pos;
     data->motion->vel = vel;
     data->motion->accel = accel;
-/*    imuDirFn[CURR_DIR].setPos(data->motion->pos);*/
+    imuDirFn[CURR_DIR].setPos(0);
 }
 
+void resetNav() {
+    data->motion->pos = 0;
+    data->motion->vel = 0;
+    data->motion->accel = 0;
+    imuDirFn[CURR_DIR].setPos(0);
+    data->motion->retroCount = 0;
+
+    /* reset rest */
+}
 
 /* TODO Open Qs
  *      1. Find out how bad drift is
  *      2. Are we going in X or Y plane
  */
 void navLoop(void *unused) {
     int lastRetroCount = 0;
+    data->motion->missedRetro = 0;
     csvFormatHeader();
     while (1) {
         if (lastRetroCount != data->motion->retroCount) {
             updateRawMotionData();
             filterMotion(FILTER_NONE);
         }
 
-        if ((imuDirFn[CURR_DIR].getPos() - data->motion->pos) > STRIP_DISTANCE * 2) {
+        if ((imuDirFn[CURR_DIR].getPos() > ((float)STRIP_DISTANCE * 1.5))) {
             /* If it is that different, we likely missed a strip */
+            data->motion->missedRetro += 1;
             fprintf(stderr, "MISSED A TAPESTRIP\n"); //FIXME All we do rn is notify, to be adjusted in test
+            imuDirFn[CURR_DIR].setPos(0);
         }
 
         if ((getuSTimestamp() - data->timers->startTime) > TOTAL_RUN_TIME) {

embedded/app/src/pressure_fault_checking.c

@@ -16,7 +16,38 @@
 
 #include "pressure_fault_checking.h"
 
-extern data_t *data;
+bool checkIdlePressures(void) {
+	if (data->pressure->primTank < PS1_BOTTOM_LIMIT_IDLE || data->pressure->primTank > PS1_TOP_LIMIT_IDLE) {
+		fprintf(stderr, "primTank pressure failing\n");
+		return false;
+	}
+	if (data->pressure->primLine < PS2_BOTTOM_LIMIT_IDLE || data->pressure->primLine > PS2_TOP_LIMIT_IDLE) {
+		fprintf(stderr, "primLine pressure failing\n");
+		return false;
+	}
+	if (data->pressure->primAct < PS3_BOTTOM_LIMIT_IDLE || data->pressure->primAct > PS3_TOP_LIMIT_IDLE) {
+		fprintf(stderr, "primAct pressure failing\n");
+		return false;
+	}
+	if (data->pressure->secTank < SEC_PS1_BOTTOM_LIMIT_IDLE || data->pressure->secTank > SEC_PS1_TOP_LIMIT_IDLE) {
+		fprintf(stderr, "Secondary primTank pressure failing\n");
+		return false;
+	}
+	if (data->pressure->secLine < SEC_PS2_BOTTOM_LIMIT_IDLE || data->pressure->secLine > SEC_PS2_TOP_LIMIT_IDLE) {
+		fprintf(stderr, "Secondary primLine pressure failing\n");
+		return false;
+	}
+	if (data->pressure->secAct < SEC_PS3_BOTTOM_LIMIT_IDLE || data->pressure->secAct > SEC_PS3_TOP_LIMIT_IDLE) {
+		fprintf(stderr, "Secondary primAct pressure failing\n");
+		return false;
+	}
+	if (data->pressure->pv < PV_BOTTOM_LIMIT || data->pressure->pv > PV_TOP_LIMIT) {
+        fprintf(stderr, "Pressure vessel depressurizing\n");
+        return false;
+    }
+  
+	return true;
+}
 
 bool checkPrerunPressures(void) {
 	if (data->pressure->primTank < PS1_BOTTOM_LIMIT_PRE || data->pressure->primTank > PS1_TOP_LIMIT_PRE) {

embedded/app/src/state_machine.c

@@ -134,9 +134,10 @@ static int initIdle(state_t *idle) {
 
     initTransition(idle->transitions[0], findState(PUMPDOWN_NAME), genTranAction);
     initTransition(idle->transitions[1], findState(NON_RUN_FAULT_NAME), genTranAction);
+    initTransition(idle->transitions[2], findState(RUN_FAULT_NAME), toRunFault);
     addTransition(IDLE_NAME, idle->transitions[0]);
     addTransition(IDLE_NAME, idle->transitions[1]);
-    
+    addTransition(IDLE_NAME, idle->transitions[2]);
     idle->fault = idle->transitions[1];
 
     return 0;
@@ -167,13 +168,11 @@ static int initPropulsion(state_t *propulsion) {
 
 static int initBraking(state_t *braking) {
 
-    initTransition(braking->transitions[0], findState(CRAWL_NAME), toCrawl);
-    initTransition(braking->transitions[1], findState(STOPPED_NAME), genTranAction);
-    initTransition(braking->transitions[2], findState(RUN_FAULT_NAME), genTranAction);
+    initTransition(braking->transitions[0], findState(STOPPED_NAME), genTranAction);
+    initTransition(braking->transitions[1], findState(RUN_FAULT_NAME), genTranAction);
     addTransition(BRAKING_NAME, braking->transitions[0]);
-    addTransition(BRAKING_NAME, braking->transitions[1]);
     addTransition(BRAKING_NAME, 
-            braking->fault = braking->transitions[2]);
+            braking->fault = braking->transitions[1]);
 
     return 0;
 }
@@ -201,7 +200,7 @@ static int initStopped(state_t *stopped) {
 
 
     initTransition(stopped->transitions[0], findState(POST_RUN_NAME), genTranAction);
-    initTransition(stopped->transitions[1], findState(CRAWL_NAME), toCrawl);
+/*    initTransition(stopped->transitions[1], findState(SERV_PRECHARGE_NAME), toServPrecharge);*/
     initTransition(stopped->transitions[2], findState(RUN_FAULT_NAME), genTranAction);
     addTransition(STOPPED_NAME, stopped->transitions[0]);
     addTransition(STOPPED_NAME, stopped->transitions[1]);
@@ -227,6 +226,7 @@ static int initSafeToApproach(state_t *safeToApproach) {
     return 0;
 }
 
+
 /***
  * findTransition - Looks through a passed in states list of transitions
  * 	and identifies the one that leads to a specified target
@@ -245,11 +245,14 @@ stateTransition_t *findTransition(state_t *srcState, char *targName) {
     }
     return NULL;
 }
-
-state_t *getCurrState(void) {
+state_t *getCurrState(void);
+state_t *getCurrState() {
     return stateMachine.currState;
 }
-
+void setCurrState(state_t *state);
+void setCurrState(state_t *state) {
+    stateMachine.currState = state;
+}
 /***
  * runStateMachine -
  *		Executes the current states action. A mini control loop
@@ -277,7 +280,6 @@ void runStateMachine(void) {
     }
     /* execute the state and check if we should be transitioning */
 	stateTransition_t *transition = stateMachine.currState->action();
-    printf("curr: %s\n", stateMachine.currState->name);
     if (transition != NULL) {
         if (transition->action() == 0) 
             stateMachine.currState = transition->target;
@@ -302,7 +304,7 @@ void buildStateMachine(void) {
         stateMachine.allStates[i] = malloc(sizeof(state_t));
     }
 
-    initState(stateMachine.allStates[0], IDLE_NAME, idleAction, 2);
+    initState(stateMachine.allStates[0], IDLE_NAME, idleAction, 4);
     initState(stateMachine.allStates[1], PUMPDOWN_NAME, pumpdownAction, 2);
     initState(stateMachine.allStates[2], PROPULSION_NAME, propulsionAction, 2);
     initState(stateMachine.allStates[3], BRAKING_NAME, brakingAction, 3);