Merge pull request #115 from badgerloop-software/steamfitters-updates

rdaruwala · web-flow · commit 17a017c96669 · 2019-11-04T18:38:11.000-06:00
Steamfitters Updates
diff --git a/embedded/app/include/states.h b/embedded/app/include/states.h
@@ -8,7 +8,7 @@
 
 /* Pressure sensor acceptable limits (in PSI) */
 #define PS1_BOTTOM_LIMIT_IDLE           -50
-#define PS1_BOTTOM_LIMIT_PRE        	0
+#define PS1_BOTTOM_LIMIT_PRE        	-50
 #define PS1_BOTTOM_LIMIT_CRAWLPOST  	300
 #define PS1_TOP_LIMIT_PRE          		1300
 #define PS1_TOP_LIMIT_CRAWLPOST     	1000
@@ -32,15 +32,15 @@
 #define PS3_TOP_LIMIT_CRAWLPOST			150
 
 #define SEC_PS1_BOTTOM_LIMIT_IDLE       -50
-#define SEC_PS1_BOTTOM_LIMIT_PRE		300
+#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		0
+#define SEC_PS2_BOTTOM_LIMIT_PRE	-10
 #define SEC_PS2_BOTTOM_LIMIT_CRAWLPOST	0
 #define SEC_PS2_TOP_LIMIT_PRE			150
 #define SEC_PS2_TOP_LIMIT_CRAWLPOST		150
diff --git a/embedded/app/main/badgerloop_HV.cpp b/embedded/app/main/badgerloop_HV.cpp
@@ -45,11 +45,13 @@ int init() {
     initHVIox(true);
 
     SetupMotor();
+
     /*    initMotor();   */
-/*    initPressureSensors();*/
+    /*    initPressureSensors();*/
+
     /* Allocate needed memory for state machine and create graph */
 	buildStateMachine();
-printf("Ezra\n");
+
     /* Init telemetry */
     SetupHVTelemetry((char *) DASHBOARD_IP, DASHBOARD_PORT);
 	SetupHVTCPServer();
@@ -58,6 +60,7 @@ printf("Ezra\n");
 	struct sigaction sig;
     sig.sa_sigaction = emergQuitter;
     sigaction(SIGINT, &sig, NULL);
+
     /* Start 'black box' data saving */
 /*    SetupDataDump();*/
 	
@@ -106,6 +109,7 @@ int main() {
         } else {
             i += 1;
         }
+	    // fprintf(stderr, "%d,%d,%d\n", data->rms->actualTorque, data->rms->motorSpeed, getuSTimestamp());
         usleep(10000);
 
 		// Control loop
diff --git a/embedded/app/main/badgerloop_LV.cpp b/embedded/app/main/badgerloop_LV.cpp
@@ -12,8 +12,7 @@ extern "C"
     #include "lv_iox.h"
     #include "nav.h"
     #include "braking.h"
-    
-#include "connStat.h"
+    #include "connStat.h"
     #include "proc_iox.h"
 	#include "imu.h"
     #include <data.h>
@@ -29,7 +28,7 @@ int init() {
     initLVIox(true);
 	
     /* Init all peripherals */
-/*	SetupIMU();  GOODBYE MY SWEET SUMMER CHILD */
+	SetupIMU();
     initRetros();
     initNav();
     
@@ -72,8 +71,7 @@ int main() {
             brakeSecondaryUnactuate();
             data->flags->brakeSecRetr = false;
         }
-		
-        
+
         if (data->state != 1 && (!checkTCPStatHV() || !checkTCPStat())) {
             printf("ERRS: %d\n", errs);
             errs += 1;
diff --git a/embedded/app/src/nav.c b/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);
 }
diff --git a/embedded/data/include/data.h b/embedded/data/include/data.h
@@ -142,7 +142,7 @@ typedef struct motion_t {
  *
  */
 typedef struct bms_t {
-    float packCurrent;
+    int64_t packCurrent;
     float packVoltage;
     int imdStatus;
     uint16_t packDCL;
diff --git a/embedded/examples/solenoidTest.c b/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;
 }
diff --git a/embedded/peripherals/include/imu.h b/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();
diff --git a/embedded/peripherals/src/bms.c b/embedded/peripherals/src/bms.c
@@ -40,10 +40,8 @@ int bmsParseMsg(uint32_t id, uint8_t *msg) {
     bms_t *bms = data->bms;
 	switch(id) {
 		case 0x6B0:		
-            bms->packCurrent = msg[1] | msg[0] << 8;
-			bms->packCurrent /= 10;
-			bms->packVoltage = msg[3] | msg[2] << 8;
-			bms->packVoltage /= 10;
+            bms->packCurrent = (msg[1] | msg[0] << 8)/10;
+			bms->packVoltage = (msg[3] | msg[2] << 8)/10;
 			bms->Soc = msg[4]/2;
 			bms->relayStatus = msg[6] | msg[5] << 8;
             bms->cellMaxVoltage = ((msg[5] << 8)| msg[6]) /10000.0;
@@ -68,8 +66,7 @@ int bmsParseMsg(uint32_t id, uint8_t *msg) {
 /*			printf("ID: 0x%3lx\r\n", (long unsigned int) id);*/
 			bms->relayStatus = msg[1] | msg[0] << 8;
 			bms->relayStatus = msg[0];
-			bms->inputVoltage = msg[2] | (msg[3] << 8);
-			bms->inputVoltage /= 10;
+			bms->inputVoltage = (msg[2] | (msg[3] << 8))/10;
 #ifdef DEBUG_BMS
             printf("Relay status %d\r\n", bms->relayStatus);
 			printf("Input Source Supply Voltage: %f\r\n", bms->inputVoltage);
@@ -109,8 +106,7 @@ int bmsParseMsg(uint32_t id, uint8_t *msg) {
 			bms->Soc /= 2;
 			bms->packResistance = msg[1] | (msg[2] << 8);
 			bms->packHealth = msg[3];
-			bms->packOpenVoltage = msg[4] | (msg[5] << 8);
-			bms->packOpenVoltage /= 10;
+			bms->packOpenVoltage = (msg[4] | (msg[5] << 8))/10;
 			bms->packCycles = msg[6] | (msg[7] << 8);
 #ifdef DEBUG_BMS
 			printf("SOC %d\r\n", bms->Soc);
@@ -121,12 +117,6 @@ int bmsParseMsg(uint32_t id, uint8_t *msg) {
 #endif
             break;
 		case 0x150:
-
-/*			bms->avgTemp = msg[0];*/
-			bms->packCurrent /= 10;
-/*            bms->cellMinVoltage = ((msg[3] << 8) | msg[4]) / 10000.0;*/
-/*			bms->packVoltage = msg[2] | (msg[3] << 8);*/
-/*			bms->packVoltage /= 10;*/
 			bms->packAh = msg[4] | (msg[5] << 8);
 			bms->highTemp = msg[6];
 			bms->lowTemp = msg[7];
diff --git a/embedded/peripherals/src/braking.c b/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();
     }
diff --git a/embedded/peripherals/src/imu.c b/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;
diff --git a/embedded/peripherals/src/rms.c b/embedded/peripherals/src/rms.c
@@ -149,7 +149,7 @@ int rmsCmdResponseParse(uint8_t *rmsData, uint16_t filter, bool write) {
 }
 int rmsSendHbMsg(uint16_t torque) {
 	
-	uint8_t payload[] = {TORQUE_SCALE_LWR(torque), 0/*TORQUE_SCALE_UPR(torque)*/, 0x0, 0x0, 
+	uint8_t payload[] = {TORQUE_SCALE_LWR(torque), TORQUE_SCALE_UPR(torque), 0x0, 0x0, 
         0x1, 0x1, 0x0, 0x0};
     
     sem_wait(&canSem);
diff --git a/middleware/src/HVTCPSocket.cpp b/middleware/src/HVTCPSocket.cpp
@@ -191,6 +191,7 @@ void *TCPLoop(void *arg)
 		// HEARTBEAT
 		if (!strncmp(buffer, "ping", MAX_COMMAND_SIZE))
 		{
+			printf("ping\n");
 			// Send acknowledge packet back
 			send(new_socket, (char *)"pong1", strlen("pong1"), 0);
 		}
@@ -226,7 +227,7 @@ void signalLV(char *cmd)
     addr.sin_port = htons(LV_SERVER_PORT);
     
     if (connect(srvFd, (SA*)&addr,addrlen) != 0) {
-        fprintf(stderr, "Failed to open port\n");
+     //   fprintf(stderr, "Failed to open port\n");
         close(srvFd);
         return;
     }
diff --git a/middleware/src/LVTCPSocket.cpp b/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)){
diff --git a/middleware/src/LVTelemetry_Loop.cpp b/middleware/src/LVTelemetry_Loop.cpp
@@ -16,6 +16,7 @@
 extern "C" 
 {
     #include "lv_iox.h"
+    #include "imu.h"
 }
 
 
@@ -77,21 +78,24 @@ void *LVTelemetryLoop(void *arg)
 			
 			// POSITION
 			Value pos;
-			pos.SetFloat(data->motion->pos);
-			
+			//pos.SetFloat(getPosX());
+            pos.SetFloat(0);
+
 			// 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;
diff --git a/middleware/src/data_dump.cpp b/middleware/src/data_dump.cpp
@@ -36,7 +36,7 @@ void currentDateTime(char *buf) {
 
 /* Thread Loop */
 void *DataLoop(void *arg){
-	
+
 	(void) arg;
 	
 	FILE *fp;
@@ -45,16 +45,18 @@ void *DataLoop(void *arg){
 	char timestamp[80];
 	currentDateTime(timestamp);
 	
-	sprintf(dir, "../data_logs/%s.csv", timestamp);
+	sprintf(dir, "../../data_logs/LOG.csv", timestamp);
 	
 	fp = fopen (dir,"w");
 	if (fp == NULL){
 		printf("Unable to open file\n");
 		return NULL;
 	}
+
 	
-	fprintf(fp, "FRP,FPD,FRC,FP,FEB,TST,TLR,TLR1,TLR2,TLR3,Pps1,Pps2,Pps3,Sps1,Sps2,Sps3,pv,pos,vel,accel,retC,pCurr,pVol,pDCL,pCCL,pRes,pHealth,pOV,pC,pAH,iV,Soc,hiT,loT,cMax,cMin,cAvg,mCells,nCells,igbtTemp,GDBT,CBT,mT,mS,paC,pbC,pcC,dcbV,lvV,cc1,cc2,fC1,fC2,cT,aT,rS,eF,dcbC,oVL\n");
 	
+	fprintf(fp, "FRP,FPD,FRC,FP,FEB,TST,TLR,TLR1,TLR2,TLR3,Pps1,Pps2,Pps3,Sps1,Sps2,Sps3,pv,pos,vel,accel,retC,pCurr,pVol,pDCL,pCCL,pRes,pHealth,pOV,pC,pAH,iV,Soc,hiT,loT,cMax,cMin,cAvg,mCells,nCells,igbtTemp,GDBT,CBT,mT,mS,paC,pbC,pcC,dcbV,lvV,cc1,cc2,fC1,fC2,cT,aT,rS,eF,dcbC,oVL\n");
+
 	while(1){
 		
 		// FLAGS

Original file line number	Diff line number	Diff line change
`@@ -58,13 +58,11 @@ void showNavData() {`
`58`	`58`	`}`
`59`	`59`
`60`	`60`	`void csvFormatHeader() {`
`61`		`- printf("posx,posy,posz,velx,vely,velz,accelx,accely,accelz,pos,vel,accel\n");`
	`61`	`+ printf("accelx,accely,accelz,pos,vel,accel\n");`
`62`	`62`	`}`
`63`	`63`
`64`	`64`	`void csvFormatShow() {`
`65`		`- 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",`
`66`		`- getPosX(), getPosY(), getPosZ(),`
`67`		`- getDeltaVX(), getDeltaVY(), getDeltaVZ(),`
	`65`	`+ printf("%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f\n",`
`68`	`66`	`getAccelX(), getAccelY(), getAccelZ(),`
`69`	`67`	`data->motion->pos, data->motion->vel, data->motion->accel);`
`70`	`68`	`}`
Original file line number	Diff line number	Diff line change
`@@ -142,7 +142,7 @@ typedef struct motion_t {`
`142`	`142`	`*`
`143`	`143`	`*/`
`144`	`144`	`typedef struct bms_t {`
`145`		`- float packCurrent;`
	`145`	`+ int64_t packCurrent;`
`146`	`146`	`float packVoltage;`
`147`	`147`	`int imdStatus;`
`148`	`148`	`uint16_t packDCL;`
Original file line number	Diff line number	Diff line change
`@@ -61,6 +61,8 @@ int main(int argc, char *argv[]) {`
`61`	`61`	`int state = whatToDo(argc, argv);`
`62`	`62`	`printf("STATE: %d\n", state);`
`63`	`63`	`if (state < 0) return -1;`
	`64`	`+ printf("start\n");`
`64`	`65`	`solenoidSet(sol, state);`
	`66`	`+ printf("end\n");`
`65`	`67`	`return 0;`
`66`	`68`	`}`
Original file line number	Diff line number	Diff line change
`@@ -97,7 +97,7 @@ static double avgDouble(double *arr, int size) {`
`97`	`97`
`98`	`98`	`int brake() {`
`99`	`99`	`brakePrimaryActuate();`
`100`		`- usleep(500000);`
	`100`	`+ usleep(5000000);`
`101`	`101`	`if (limSwitchGet(PRIM_LIM_SWITCH)) {`
`102`	`102`	`brakeSecondaryActuate();`
`103`	`103`	`}`