Pushing for rohan

Author: Ezra Boley

embedded/app/include/motor.h

@@ -16,7 +16,7 @@ void setTorque(uint16_t val);
 uint16_t getTorque(void);
 
 void setMotorIsOn(bool val);
-
+void setMotorCrawl(void);
 bool getMotorIsOn(void);
 
 int startMotor(void);

embedded/app/include/state_machine.h

@@ -19,8 +19,8 @@
 #define PROPULSION_NAME         "propulsion"
 #define BRAKING_NAME            "braking"
 #define STOPPED_NAME            "stopped"
-#define SERV_PRECHARGE_NAME     "servicePrecharge"
-#define CRAWL_NAME              "servicePropulsion"
+#define SERV_PRECHARGE_NAME     "crawlPrecharge"
+#define CRAWL_NAME              "crawl"
 #define POST_RUN_NAME           "postRun"
 #define SAFE_TO_APPROACH_NAME   "safeToApproach"
 
@@ -35,6 +35,7 @@ typedef struct stateTransition_t stateTransition_t;
 typedef struct stateMachine_t stateMachine_t;
 
 
+extern stateTransition_t *runFault, *nonRunFault;
 state_t *getCurrState(void);
 
 state_t *setCurrState(state_t *state);
@@ -66,7 +67,8 @@ typedef struct state_t {
 	char *name; // FIXME Thinking about switching this to a number
 	stateTransition_t **transitions;
     stateTransition_t *fault;
-    int (*intro)(void);
+    stateTransition_t *next;
+    int (*begin)(void);
     int numTransitions;
     int transitionCounter;
 } state_t;
@@ -75,6 +77,7 @@ typedef struct stateMachine_t {
 	state_t *currState;
     char *  overrideStateName;
 	state_t **allStates;
+    uint64_t start;
 } stateMachine_t;
 
 #endif

embedded/app/include/states.h

@@ -8,39 +8,39 @@
 
 /* Pressure sensor acceptable limits (in PSI) */
 #define PS1_BOTTOM_LIMIT_IDLE           -50
-#define PS1_BOTTOM_LIMIT_PRE        	500
+#define PS1_BOTTOM_LIMIT_PRE        	0
 #define PS1_BOTTOM_LIMIT_CRAWLPOST  	300
 #define PS1_TOP_LIMIT_PRE          		1300
-#define PS1_TOP_LIMIT_CRAWLPOST     	50
+#define PS1_TOP_LIMIT_CRAWLPOST     	1000
 #define PS1_TOP_LIMIT_IDLE           	1300
 
 #define PS2_BOTTOM_LIMIT_IDLE        	-15
-#define PS2_BOTTOM_LIMIT_PRE        	125
+#define PS2_BOTTOM_LIMIT_PRE        	-5
 #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 PS2_TOP_LIMIT_CRAWLPOST     	100
 
 
 #define PS3_BOTTOM_LIMIT_IDLE        	-10
 #define PS3_BOTTOM_LIMIT_PRE        	-15
-#define PS3_BOTTOM_LIMIT_BRAKING		120
+#define PS3_BOTTOM_LIMIT_BRAKING		-10
 #define PS3_BOTTOM_LIMIT_CRAWLPOST		-15
 #define PS3_TOP_LIMIT_IDLE        	    15
-#define PS3_TOP_LIMIT_PRE           	15
+#define PS3_TOP_LIMIT_PRE           	150
 #define PS3_TOP_LIMIT_BRAKING			150
 #define PS3_TOP_LIMIT_CRAWLPOST			150
 
 #define SEC_PS1_BOTTOM_LIMIT_IDLE       -50
-#define SEC_PS1_BOTTOM_LIMIT_PRE		600
+#define SEC_PS1_BOTTOM_LIMIT_PRE		300
 #define SEC_PS1_BOTTOM_LIMIT_CRAWLPOST	300
 #define SEC_PS1_TOP_LIMIT_PRE			1400
 #define SEC_PS1_TOP_LIMIT_CRAWLPOST		1400
 #define SEC_PS1_TOP_LIMIT_IDLE          1400
 
 
 #define SEC_PS2_BOTTOM_LIMIT_IDLE       -15
-#define SEC_PS2_BOTTOM_LIMIT_PRE		120
+#define SEC_PS2_BOTTOM_LIMIT_PRE		0
 #define SEC_PS2_BOTTOM_LIMIT_CRAWLPOST	0
 #define SEC_PS2_TOP_LIMIT_PRE			150
 #define SEC_PS2_TOP_LIMIT_CRAWLPOST		20
@@ -113,18 +113,18 @@
 #define DC_BUS_VOLTAGE_MAX			360
 
 #define DC_BUS_CURRENT_MIN			-5
-#define DC_BUS_CURRENT_MAX_IDLE		319
-#define DC_BUS_CURRENT_MAX_PUMPDOWN	2
+#define DC_BUS_CURRENT_MAX_IDLE		10
+#define DC_BUS_CURRENT_MAX_PUMPDOWN	319
 #define DC_BUS_CURRENT_MAX_PROP		319
-#define DC_BUS_CURRENT_MAX_BRAKING	0.01
-#define DC_BUS_CURRENT_MAX_POST		0.01
+#define DC_BUS_CURRENT_MAX_BRAKING	319
+#define DC_BUS_CURRENT_MAX_POST		319
 #define DC_BUS_CURRENT_MAX_CRAWL	200
 
-#define LV_VOLTAGE_MIN				10
+#define LV_VOLTAGE_MIN				8
 #define LV_VOLTAGE_MAX				14
 
 #define CMD_TORQUE_MIN				0
-#define CMD_TORQUE_MAX_PRE			0
+#define CMD_TORQUE_MAX_PRE			5
 #define CMD_TORQUE_MAX_RUN			90
 #define CMD_TORQUE_MAX_POST			0
 #define CMD_TORQUE_MAX_CRAWL		40

embedded/app/include/transitions.h

@@ -1,17 +1,24 @@
 #ifndef __TRANSITIONS_H__
 #define __TRANSITIONS_H__
 
+int genIdle(void);
+
 int genTranAction(void);
 
-int toPropulsion(void);
+int genPropulsion(void);
+
+int genPumpdown(void);
+
+int genBraking(void);
+
+int genCrawl(void);
 
-int toPumpdown(void);
+int genRunFault(void);
 
-int toBraking(void);
+int genServPrecharge(void);
 
-int toCrawl(void);
+int genPostRun(void);
 
-int toRunFault(void);
+int genStopped(void);
 
-int toServPrecharge(void);
 #endif

embedded/app/main/badgerloop_HV.cpp

@@ -66,13 +66,13 @@ int init() {
 
 int main() {
 	/* Create the big data structures to house pod data */
-	
-	if (init() == 1) {
+	int i = 0;
+	char buffer[100];
+    
+    if (init() == 1) {
 		fprintf(stderr, "Error in initialization! Exiting...\r\n");
 		exit(1);
 	}
-/*    signal(SIGINT, shutdown);*/
-    printf("Here\n");
 
 	while(1) {
 	    runStateMachine();
@@ -81,8 +81,10 @@ int main() {
 /*                checkTCPStat(),*/
 /*                checkUDPStat());*/
         if (data->flags->shouldBrake) {
+            printf("signalling\n");
             signalLV((char *)"brake");
             data->flags->shouldBrake = false;
+            printf("signallingDone\n");
         }
         if (data->flags->brakeInit) {
             signalLV((char *)"primBrakeOff");
@@ -96,6 +98,13 @@ int main() {
             data->flags->clrMotionData = false;
         }
 
+        if (i >= 50) {
+            sprintf(buffer, "state%d\n", data->state == 1);
+            signalLV((char *) buffer);
+            i = 0;
+        } else {
+            i += 1;
+        }
         usleep(10000);
 
 		// Control loop

embedded/app/main/badgerloop_LV.cpp

@@ -28,7 +28,7 @@ int init() {
     initLVIox(true);
 
     /* Init all peripherals */
-	SetupIMU();
+/*	SetupIMU();  GOODBYE MY SWEET SUMMER CHILD */
     initRetros();
     initPressureMonitor();
     initNav();
@@ -37,7 +37,7 @@ int init() {
 
     SetupLVTelemetry((char *) DASHBOARD_IP, DASHBOARD_PORT);
 	SetupLVTCPServer();
-
+    data->state = 1;
     return 0;
 }
 
@@ -50,7 +50,7 @@ int main() {
     int errs = 0;
     while(1) {
 		usleep(100000);
-/*        if (errs > 100) brake();*/
+        if (errs > 50) brake();
 
         if (data->flags->shouldBrake) {
             brake();
@@ -72,11 +72,14 @@ int main() {
             brakeSecondaryUnactuate();
             data->flags->brakeSecRetr = false;
         }
-		if (!checkUDPStat() || !checkTCPStat())
+		
+        
+        if (data->state != 1 && (!checkTCPStatHV() || !checkTCPStat())) {
+            printf("ERRS: %d\n", errs);
             errs += 1;
-        else 
+        } else { 
             errs = 0;
-        
+        }
 
 
         // Control loop

embedded/app/src/motor.c

@@ -56,9 +56,9 @@ void SetupMotor() {
 static void motorHbLoop() {
     while(1) {
         if (motorEnabled)
-            rmsInvEn();
+            rmsSendHbMsg(2);
         else if (lowTorqueMode)
-            rmsInvEnNoTorque();
+            rmsSendHbMsg(2);
         else
             rmsIdleHb();
         usleep(10000);

embedded/app/src/nav.c

@@ -15,26 +15,6 @@
 #define TOTAL_RUN_TIME 30   /* s */
 #define EXPECTED_DECEL 9.8  /* m/s/s */
 
-/* Just for test */
-#define X_DIR   0
-#define Y_DIR   1
-#define Z_DIR   2
-
-/* Change this to change which direction the IMU thinks is right */
-#define CURR_DIR X_DIR
-
-struct dirFn_t {
-    float (*getPos)(void);   
-    void  (*setPos)(float val);
-};
-
-static struct dirFn_t imuDirFn[] =
-{
-    {getPosX, setPosX},
-    {getPosY, setPosY},
-    {getPosZ, setPosZ}
-};
-
 #define WINDOW_SIZE    2
 
 static pthread_t navThread;
@@ -149,14 +129,13 @@ void filterMotion(int filterType) {
     data->motion->pos = pos;
     data->motion->vel = vel;
     data->motion->accel = accel;
-    imuDirFn[CURR_DIR].setPos(0);
 }
+
 void resetNav();
 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 */
@@ -176,17 +155,6 @@ void navLoop(void *unused) {
             filterMotion(FILTER_NONE);
         }
 
-        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) {*/
-/*            data->flags->shouldStop = true;*/
-/*        }*/
-
         lastRetroCount = data->motion->retroCount;
     /*    showNavData();   */
     /*    csvFormatShow(); */

embedded/app/src/pressure_fault_checking.c

@@ -17,7 +17,7 @@
 #include "pressure_fault_checking.h"
 
 bool checkIdlePressures(void) {
-	if (data->pressure->primTank < PS1_BOTTOM_LIMIT_IDLE || data->pressure->primTank > PS1_TOP_LIMIT_IDLE) {
+    if (data->pressure->primTank < PS1_BOTTOM_LIMIT_IDLE || data->pressure->primTank > PS1_TOP_LIMIT_IDLE) {
 		fprintf(stderr, "primTank pressure failing\n");
 		return false;
 	}
@@ -50,7 +50,7 @@ bool checkIdlePressures(void) {
 }
 
 bool checkPrerunPressures(void) {
-	if (data->pressure->primTank < PS1_BOTTOM_LIMIT_PRE || data->pressure->primTank > PS1_TOP_LIMIT_PRE) {
+    if (data->pressure->primTank < PS1_BOTTOM_LIMIT_PRE || data->pressure->primTank > PS1_TOP_LIMIT_PRE) {
 		fprintf(stderr, "primTank pressure failing\n");
 		return false;
 	}
@@ -83,7 +83,7 @@ bool checkPrerunPressures(void) {
 }
 
 bool checkBrakingPressures(void) {
-	if (data->pressure->primTank < PS1_BOTTOM_LIMIT_PRE || data->pressure->primTank > PS1_TOP_LIMIT_PRE) {
+    if (data->pressure->primTank < PS1_BOTTOM_LIMIT_PRE || data->pressure->primTank > PS1_TOP_LIMIT_PRE) {
 		fprintf(stderr, "primTank pressure failing\n");
 		return false;
 	}
@@ -117,7 +117,7 @@ bool checkBrakingPressures(void) {
 }
 
 bool checkCrawlPostrunPressures(void) {
-	if (data->pressure->primTank < PS1_BOTTOM_LIMIT_CRAWLPOST || data->pressure->primTank > PS1_TOP_LIMIT_CRAWLPOST) {
+    if (data->pressure->primTank < PS1_BOTTOM_LIMIT_CRAWLPOST || data->pressure->primTank > PS1_TOP_LIMIT_CRAWLPOST) {
 		fprintf(stderr, "primTank pressure failing\n");
 		return false;
 	}