Got the motor to spin with the brakes not actuated

Author: Ezra Boley

embedded/app/include/motor.h

@@ -25,4 +25,9 @@ int stopMotor(void);
 
 void idleMotor(void);
 
+void setMotorEn();
+void clrMotorEn();
+void SetupMotor();
+
+
 #endif

embedded/app/include/transitions.h

@@ -5,6 +5,8 @@ int genTranAction(void);
 
 int toPropulsion(void);
 
+int toPumpdown(void);
+
 int toBraking(void);
 
 int toCrawl(void);

embedded/app/main/badgerloop_HV.cpp

@@ -23,13 +23,9 @@ extern "C"
     #include "state_machine.h"
     #include "NCD9830DBR2G.h"
 }
-extern bool noTorqueMode;
-extern bool motorIsEnabled;
 void emergQuitter(int sig, siginfo_t* inf, void *nul) {
     printf("shutdown\n");
-    motorIsEnabled = false;
-    noTorqueMode = false;
-    usleep(10000);
+    setMCUHVEnabled(false);
     rmsCmdNoTorque();
     sleep(1);
     rmsDischarge();
@@ -46,7 +42,9 @@ int init() {
     SetupCANDevices();
     initProcIox(true);
     initHVIox(true);
-/*    initMotor();   */
+
+    SetupMotor();
+    /*    initMotor();   */
     initPressureSensors();
     /* Allocate needed memory for state machine and create graph */
 	buildStateMachine();
@@ -65,11 +63,6 @@ int init() {
     return 0;	
 }
 
-void shutdown() {
-    setTorque(0);   /* Try our best to de-escalate while not overstaying our welcome */
-    data->flags->shutdown = true;
-    return;
-}
 int main() {
 	/* Create the big data structures to house pod data */
 
@@ -81,11 +74,6 @@ int main() {
     printf("Here\n");
 
 	while(1) {
-	    if (data->flags->shutdown) {
-            if (getMotorIsOn())
-                stopMotor();
-            exit(0);
-	    }
 	    runStateMachine();
         if (data->flags->shouldBrake) {
             signalLV((char *)"brake");
@@ -97,6 +85,7 @@ int main() {
             data->flags->brakeInit = false;
         }
         if (data->flags->clrMotionData) {
+            printf("signal clear\n");
             signalLV((char *) "clrMotion");
             data->flags->clrMotionData = false;
         }

embedded/app/main/badgerloop_LV.cpp

@@ -45,28 +45,27 @@ int main() {
 		exit(1);
 	}
     flags_t *f = data->flags;
-	if (f == NULL) printf("its null\n");
     while(1) {
 		usleep(100000);
-        if (f->shouldBrake) {
+        if (data->flags->shouldBrake) {
             brake();
-            f->shouldBrake = false;
+            data->flags->shouldBrake = false;
         }
-        if (f->brakePrimAct) {
+        if (data->flags->brakePrimAct) {
             brakePrimaryActuate();
-            f->brakePrimAct = false;
+            data->flags->brakePrimAct = false;
         }
-        if (f->brakePrimRetr) {
+        if (data->flags->brakePrimRetr) {
             brakePrimaryUnactuate();
-            f->brakePrimRetr = false;
+            data->flags->brakePrimRetr = false;
         }
-        if (f->brakeSecAct) {
+        if (data->flags->brakeSecAct) {
             brakeSecondaryActuate();
-            f->brakeSecAct = false;
+            data->flags->brakeSecAct = false;
         }
-        if (f->brakeSecRetr) {
+        if (data->flags->brakeSecRetr) {
             brakeSecondaryUnactuate();
-            f->brakeSecRetr = false;
+            data->flags->brakeSecRetr = false;
         }
 		// Control loop
 	}

embedded/app/src/motor.c

@@ -8,7 +8,7 @@
 #include <unistd.h>
 #include <hv_iox.h>
 #include <data.h>
-
+bool invEn = false;
 /***
  * The high level interface for the motor
  */
@@ -55,6 +55,37 @@ int initMotor() {
     return 0;
 }
 
+static pthread_t hbThread;
+static bool motorEnabled = false;
+static bool noTorqueMode = false;
+static void motorHbLoop();
+
+void setMotorEn() {
+    motorEnabled = true;
+}
+
+void clrMotorEn() {
+    motorEnabled = false;
+}
+
+
+
+void SetupMotor() {
+    pthread_create(&hbThread, NULL, (motorHbLoop), NULL);
+}
+
+static void motorHbLoop() {
+    while(1) {
+        if (motorEnabled)
+            rmsInvEn();
+        else if (noTorqueMode)
+            rmsInvEnNoTorque();
+        else
+            rmsIdleHb();
+        usleep(10000);
+    }
+}
+
 void joinMotorHbThread() {
     JOIN_HB_THREAD = true;
     if (pthread_join(hbLoop, NULL) != 0) {
@@ -102,6 +133,7 @@ int startMotor() {
 	rmsClrFaults();
 	rmsInvDis();
 /*	idleMotor();*/
+/*    invEn = true;*/
     sleep(3); /* Not ideal, but unless we have a way to check status this stays */
     rmsInvEnNoTorque();
 	setMotorIsOn(true);
@@ -139,12 +171,14 @@ static void *motorHeartbeatLoop(void *unusedParam) {
 
 		sem_wait(&hbSem);
         if (getMotorIsOn()) {
-            printf("MOTOR IS ON\n");
+            printf("CURR TIME: %llu\n", getuSTimestamp());
             rmsInvEn();
 /*			if (rmsSendHbMsg(getTorque()) != 0) {*/
 /*                fprintf(stderr, "Failed to send heartbeat\n");*/
 /*            }*/
-        } else {
+/*        } else if (invEn) {*/
+/*            rmsInvEnNoTorque();*/
+        }else {
             if (rmsIdleHb() != 0) {
                 fprintf(stderr, "Failed to send idle heartbeat\n");
             }

embedded/app/src/nav.c

@@ -182,13 +182,10 @@ void navLoop(void *unused) {
             imuDirFn[CURR_DIR].setPos(0);
         }
 
-        if ((getuSTimestamp() - data->timers->startTime) > TOTAL_RUN_TIME) {
-            data->flags->shouldStop = true;
-        }
+/*        if ((getuSTimestamp() - data->timers->startTime) > TOTAL_RUN_TIME) {*/
+/*            data->flags->shouldStop = true;*/
+/*        }*/
 
-        if (data->flags->shouldBrake) {
-            printf("BRAKE\n");
-        }
         lastRetroCount = data->motion->retroCount;
     /*    showNavData();   */
     /*    csvFormatShow(); */

embedded/app/src/state_machine.c

@@ -132,7 +132,7 @@ static int addTransition(char *stateName, stateTransition_t *trans) {
 
 static int initIdle(state_t *idle) {
 
-    initTransition(idle->transitions[0], findState(PUMPDOWN_NAME), genTranAction);
+    initTransition(idle->transitions[0], findState(PUMPDOWN_NAME), toPumpdown);
     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]);
@@ -265,11 +265,10 @@ state_t *getCurrState() {
 void runStateMachine(void) {
     /* The cmd receiver will populate this field if we get an override */
     if (strcmp(stateMachine.overrideStateName, BLANK_NAME) != 0) {
-    
         state_t *tempState = findState(stateMachine.overrideStateName);
         /* TODO We also need to execute a transition if it exists here */
         if (tempState != NULL) {
-            stateTransition_t *trans = findTransition(tempState, stateMachine.overrideStateName);
+            stateTransition_t *trans = findTransition(stateMachine.currState, stateMachine.overrideStateName);
             if (trans != NULL) trans->action();
             stateMachine.currState = tempState;
         }
@@ -304,7 +303,7 @@ void buildStateMachine(void) {
     initState(stateMachine.allStates[0], IDLE_NAME, idleAction, 3);
     initState(stateMachine.allStates[1], PUMPDOWN_NAME, pumpdownAction, 2);
     initState(stateMachine.allStates[2], PROPULSION_NAME, propulsionAction, 2);
-    initState(stateMachine.allStates[3], BRAKING_NAME, brakingAction, 3);
+    initState(stateMachine.allStates[3], BRAKING_NAME, brakingAction, 2);
     initState(stateMachine.allStates[4], SERV_PRECHARGE_NAME, servPrechargeAction, 2);
     initState(stateMachine.allStates[5], CRAWL_NAME, crawlAction, 3);
     initState(stateMachine.allStates[6], STOPPED_NAME, stoppedAction, 3);

embedded/app/src/states.c

@@ -109,9 +109,20 @@ stateTransition_t * pumpdownAction() {
     // First check for nominal values?
     data->state = 2;
      /* Check IMD status */
+    static firstRun = true;
+    uint64_t start;
+    if (firstRun) {
+        start = getuSTimestamp();
+        firstRun = false;
+    }
+    
+/*    if (getuSTimestamp() - start > 3000000)*/
+/*        return findTransition(stateMachine.currState,PROPULSION_NAME);*/
+/**/
     if (data->flags->emergencyBrake) {
         return findTransition(stateMachine.currState ,RUN_FAULT_NAME);
     } 
+
 #ifndef NO_FAULT
     if (!getIMDStatus()) {
         return stateMachine.currState->fault;
@@ -126,7 +137,6 @@ stateTransition_t * pumpdownAction() {
     if(!checkPrerunPressures()){
         return stateMachine.currState->fault;
     }
-#endif    
     // TODO check LV Power
     // TODO check LV Temp
 
@@ -137,18 +147,18 @@ stateTransition_t * pumpdownAction() {
     if(!checkPrerunRMS()){
         return stateMachine.currState->fault;
     }
+#endif 
 
-    
-    if(data->flags->readyCommand){
-	return findTransition(stateMachine.currState, PROPULSION_NAME);
-    }
+/*    if(data->flags->readyCommand){*/
+/*	return findTransition(stateMachine.currState, PROPULSION_NAME);*/
+/*    }*/
 
     return NULL;
 }
 
 stateTransition_t * propulsionAction() {
     static bool isInit = false;
-    
+    setMotorEn();
     data->state = 3;
     if (!isInit) {
         data->flags->clrMotionData = true;
@@ -209,11 +219,13 @@ stateTransition_t * propulsionAction() {
 }
 
 stateTransition_t * brakingAction() {
+    
     data->state = 4;
     // TODO Do we differenciate between primary and secondary braking systems?
     // TODO Add logic to handle switches / actuate both
  /* Check IMD status */
     if (data->flags->emergencyBrake) {
+        printf("?\n");
         return findTransition(stateMachine.currState ,RUN_FAULT_NAME);
     } 
 #ifndef NO_FAULT
@@ -243,9 +255,9 @@ stateTransition_t * brakingAction() {
         return stateMachine.currState->fault;
     }
 #endif
-    if ((getuSTimestamp() - data->timers->startTime) > MAX_RUN_TIME){
-        return stateMachine.currState->fault;
-    }
+/*    if ((getuSTimestamp() - data->timers->startTime) > MAX_RUN_TIME){*/
+/*        return stateMachine.currState->fault;*/
+/*    }*/
 
     // CHECK TRANSITION CRITERIA
 /*    if(checkStopped()){

embedded/app/src/transitions.c

@@ -1,25 +1,55 @@
+#include <hv_iox.h>
 #include <stdio.h>
 #include <stdbool.h>
 #include <motor.h>
 #include <data.h>
 #include <transitions.h>
 #include <braking.h>
+#include <rms.h>
+
 
 /* If there is nothing special to do */
 int genTranAction() {
     return 0;
 }
 
+int toPumpdown() {
+    printf("LETS GO\n");
+    setMCULatch(true);
+    usleep(10000);
+    setMCULatch(false);
+    setMCUHVEnabled(true);
+    sleep(1);
+    if(rmsEnHeartbeat() != 0) printf("EEERR0\n");
+    if (rmsClrFaults() != 0) printf("eeE1\n");
+    if(rmsInvDis() != 0) printf("EEERR2\n");
+    return 0;
+/*    sleep(3);*/
+}
+
 int toPropulsion() {
 /*    if (startMotor() != 0) return 1;*/
 /*    setTorque(FULL_SPEED_AHEAD);*/
     data->timers->startTime = getuSTimestamp();
+    
     /* FIXME  I need a way to tell if this was successful */    
     return 0;
 }
 
+
+
 int toBraking() {
 /*    if (stopMotor() != 0) return 1;*/
+    clrMotorEn();
+    usleep(1000);
+    rmsCmdNoTorque();
+    usleep(1000);
+    rmsDischarge();
+    usleep(1000);
+    rmsInvDis();
+    usleep(1000);
+   
+    setMCUHVEnabled(false);
     brakeHV();
     return 0;
 }
@@ -29,7 +59,7 @@ int toCrawl() {
     brakeSecondaryUnactuate();    /* Usually doesnt do anything */
 
     data->timers->crawlTimer = getuSTimestamp();
-
+    
     if (startMotor() != 0) return 1;
     setTorque(CRAWL_TORQUE);
     return 0;

embedded/peripherals/src/can_devices.c

@@ -14,7 +14,7 @@ pthread_t CANThread;
 
 void SetupCANDevices(){
 	initCan();
-	initMotor();
+/*	initMotor();*/
     if (pthread_create(&CANThread, NULL, CANLoop, NULL)){
 		fprintf(stderr, "Error creating CAN thread\n");
 	}