Skip to content

Commit 17a017c

Browse files
rdaruwalardaruwala
authored
Merge pull request #115 from badgerloop-software/steamfitters-updates
Steamfitters Updates
2 parents fb41f39 + 8426b74 commit 17a017c

File tree

15 files changed

+64
-40
lines changed

15 files changed

+64
-40
lines changed

‎embedded/app/include/states.h

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88

99
/* Pressure sensor acceptable limits (in PSI) */
1010
#define PS1_BOTTOM_LIMIT_IDLE -50
11-
#define PS1_BOTTOM_LIMIT_PRE 0
11+
#define PS1_BOTTOM_LIMIT_PRE -50
1212
#define PS1_BOTTOM_LIMIT_CRAWLPOST 300
1313
#define PS1_TOP_LIMIT_PRE 1300
1414
#define PS1_TOP_LIMIT_CRAWLPOST 1000
@@ -32,15 +32,15 @@
3232
#define PS3_TOP_LIMIT_CRAWLPOST 150
3333

3434
#define SEC_PS1_BOTTOM_LIMIT_IDLE -50
35-
#define SEC_PS1_BOTTOM_LIMIT_PRE 300
35+
#define SEC_PS1_BOTTOM_LIMIT_PRE -300
3636
#define SEC_PS1_BOTTOM_LIMIT_CRAWLPOST 300
3737
#define SEC_PS1_TOP_LIMIT_PRE 1400
3838
#define SEC_PS1_TOP_LIMIT_CRAWLPOST 1400
3939
#define SEC_PS1_TOP_LIMIT_IDLE 1400
4040

4141

4242
#define SEC_PS2_BOTTOM_LIMIT_IDLE -15
43-
#define SEC_PS2_BOTTOM_LIMIT_PRE 0
43+
#define SEC_PS2_BOTTOM_LIMIT_PRE -10
4444
#define SEC_PS2_BOTTOM_LIMIT_CRAWLPOST 0
4545
#define SEC_PS2_TOP_LIMIT_PRE 150
4646
#define SEC_PS2_TOP_LIMIT_CRAWLPOST 150

‎embedded/app/main/badgerloop_HV.cpp

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -45,11 +45,13 @@ int init() {
4545
initHVIox(true);
4646

4747
SetupMotor();
48+
4849
/* initMotor(); */
49-
/* initPressureSensors();*/
50+
/* initPressureSensors();*/
51+
5052
/* Allocate needed memory for state machine and create graph */
5153
buildStateMachine();
52-
printf("Ezra\n");
54+
5355
/* Init telemetry */
5456
SetupHVTelemetry((char *) DASHBOARD_IP, DASHBOARD_PORT);
5557
SetupHVTCPServer();
@@ -58,6 +60,7 @@ printf("Ezra\n");
5860
struct sigaction sig;
5961
sig.sa_sigaction = emergQuitter;
6062
sigaction(SIGINT, &sig, NULL);
63+
6164
/* Start 'black box' data saving */
6265
/* SetupDataDump();*/
6366

@@ -106,6 +109,7 @@ int main() {
106109
} else {
107110
i += 1;
108111
}
112+
// fprintf(stderr, "%d,%d,%d\n", data->rms->actualTorque, data->rms->motorSpeed, getuSTimestamp());
109113
usleep(10000);
110114

111115
// Control loop

‎embedded/app/main/badgerloop_LV.cpp

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,7 @@ extern "C"
1212
#include "lv_iox.h"
1313
#include "nav.h"
1414
#include "braking.h"
15-
16-
#include "connStat.h"
15+
#include "connStat.h"
1716
#include "proc_iox.h"
1817
#include "imu.h"
1918
#include <data.h>
@@ -29,7 +28,7 @@ int init() {
2928
initLVIox(true);
3029

3130
/* Init all peripherals */
32-
/* SetupIMU(); GOODBYE MY SWEET SUMMER CHILD */
31+
SetupIMU();
3332
initRetros();
3433
initNav();
3534

@@ -72,8 +71,7 @@ int main() {
7271
brakeSecondaryUnactuate();
7372
data->flags->brakeSecRetr = false;
7473
}
75-
76-
74+
7775
if (data->state != 1 && (!checkTCPStatHV() || !checkTCPStat())) {
7876
printf("ERRS: %d\n", errs);
7977
errs += 1;

‎embedded/app/src/nav.c

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -58,13 +58,11 @@ void showNavData() {
5858
}
5959

6060
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");
6262
}
6363

6464
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",
6866
getAccelX(), getAccelY(), getAccelZ(),
6967
data->motion->pos, data->motion->vel, data->motion->accel);
7068
}

‎embedded/data/include/data.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -142,7 +142,7 @@ typedef struct motion_t {
142142
*
143143
*/
144144
typedef struct bms_t {
145-
float packCurrent;
145+
int64_t packCurrent;
146146
float packVoltage;
147147
int imdStatus;
148148
uint16_t packDCL;

‎embedded/examples/solenoidTest.c

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,8 @@ int main(int argc, char *argv[]) {
6161
int state = whatToDo(argc, argv);
6262
printf("STATE: %d\n", state);
6363
if (state < 0) return -1;
64+
printf("start\n");
6465
solenoidSet(sol, state);
66+
printf("end\n");
6567
return 0;
6668
}

‎embedded/peripherals/include/imu.h

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,10 @@ float getDeltaVX();
5454
float getDeltaVY();
5555
float getDeltaVZ();
5656

57+
float getVelX();
58+
float getVelY();
59+
float getVelZ();
60+
5761
void getAccelData(float *fData);
5862
float getAccelX();
5963
float getAccelY();

‎embedded/peripherals/src/bms.c

Lines changed: 4 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -40,10 +40,8 @@ int bmsParseMsg(uint32_t id, uint8_t *msg) {
4040
bms_t *bms = data->bms;
4141
switch(id) {
4242
case 0x6B0:
43-
bms->packCurrent = msg[1] | msg[0] << 8;
44-
bms->packCurrent /= 10;
45-
bms->packVoltage = msg[3] | msg[2] << 8;
46-
bms->packVoltage /= 10;
43+
bms->packCurrent = (msg[1] | msg[0] << 8)/10;
44+
bms->packVoltage = (msg[3] | msg[2] << 8)/10;
4745
bms->Soc = msg[4]/2;
4846
bms->relayStatus = msg[6] | msg[5] << 8;
4947
bms->cellMaxVoltage = ((msg[5] << 8)| msg[6]) /10000.0;
@@ -68,8 +66,7 @@ int bmsParseMsg(uint32_t id, uint8_t *msg) {
6866
/* printf("ID: 0x%3lx\r\n", (long unsigned int) id);*/
6967
bms->relayStatus = msg[1] | msg[0] << 8;
7068
bms->relayStatus = msg[0];
71-
bms->inputVoltage = msg[2] | (msg[3] << 8);
72-
bms->inputVoltage /= 10;
69+
bms->inputVoltage = (msg[2] | (msg[3] << 8))/10;
7370
#ifdef DEBUG_BMS
7471
printf("Relay status %d\r\n", bms->relayStatus);
7572
printf("Input Source Supply Voltage: %f\r\n", bms->inputVoltage);
@@ -109,8 +106,7 @@ int bmsParseMsg(uint32_t id, uint8_t *msg) {
109106
bms->Soc /= 2;
110107
bms->packResistance = msg[1] | (msg[2] << 8);
111108
bms->packHealth = msg[3];
112-
bms->packOpenVoltage = msg[4] | (msg[5] << 8);
113-
bms->packOpenVoltage /= 10;
109+
bms->packOpenVoltage = (msg[4] | (msg[5] << 8))/10;
114110
bms->packCycles = msg[6] | (msg[7] << 8);
115111
#ifdef DEBUG_BMS
116112
printf("SOC %d\r\n", bms->Soc);
@@ -121,12 +117,6 @@ int bmsParseMsg(uint32_t id, uint8_t *msg) {
121117
#endif
122118
break;
123119
case 0x150:
124-
125-
/* bms->avgTemp = msg[0];*/
126-
bms->packCurrent /= 10;
127-
/* bms->cellMinVoltage = ((msg[3] << 8) | msg[4]) / 10000.0;*/
128-
/* bms->packVoltage = msg[2] | (msg[3] << 8);*/
129-
/* bms->packVoltage /= 10;*/
130120
bms->packAh = msg[4] | (msg[5] << 8);
131121
bms->highTemp = msg[6];
132122
bms->lowTemp = msg[7];

‎embedded/peripherals/src/braking.c

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -97,7 +97,7 @@ static double avgDouble(double *arr, int size) {
9797

9898
int brake() {
9999
brakePrimaryActuate();
100-
usleep(500000);
100+
usleep(5000000);
101101
if (limSwitchGet(PRIM_LIM_SWITCH)) {
102102
brakeSecondaryActuate();
103103
}

‎embedded/peripherals/src/imu.c

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -236,6 +236,27 @@ float getDeltaVZ(){
236236
return ans;
237237
}
238238

239+
float getVelX(){
240+
sem_wait(&data->mutex);
241+
float ans = data->velX;
242+
sem_post(&data->mutex);
243+
return ans;
244+
}
245+
246+
float getVelY(){
247+
sem_wait(&data->mutex);
248+
float ans = data->velY;
249+
sem_post(&data->mutex);
250+
return ans;
251+
}
252+
253+
float getVelZ(){
254+
sem_wait(&data->mutex);
255+
float ans = data->velZ;
256+
sem_post(&data->mutex);
257+
return ans;
258+
}
259+
239260
float getAccelX(){
240261
sem_wait(&data->mutex);
241262
float ans = data->accelX;

‎embedded/peripherals/src/rms.c

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -149,7 +149,7 @@ int rmsCmdResponseParse(uint8_t *rmsData, uint16_t filter, bool write) {
149149
}
150150
int rmsSendHbMsg(uint16_t torque) {
151151

152-
uint8_t payload[] = {TORQUE_SCALE_LWR(torque), 0/*TORQUE_SCALE_UPR(torque)*/, 0x0, 0x0,
152+
uint8_t payload[] = {TORQUE_SCALE_LWR(torque), TORQUE_SCALE_UPR(torque), 0x0, 0x0,
153153
0x1, 0x1, 0x0, 0x0};
154154

155155
sem_wait(&canSem);

‎middleware/src/HVTCPSocket.cpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -191,6 +191,7 @@ void *TCPLoop(void *arg)
191191
// HEARTBEAT
192192
if (!strncmp(buffer, "ping", MAX_COMMAND_SIZE))
193193
{
194+
printf("ping\n");
194195
// Send acknowledge packet back
195196
send(new_socket, (char *)"pong1", strlen("pong1"), 0);
196197
}
@@ -226,7 +227,7 @@ void signalLV(char *cmd)
226227
addr.sin_port = htons(LV_SERVER_PORT);
227228

228229
if (connect(srvFd, (SA*)&addr,addrlen) != 0) {
229-
fprintf(stderr, "Failed to open port\n");
230+
// fprintf(stderr, "Failed to open port\n");
230231
close(srvFd);
231232
return;
232233
}

‎middleware/src/LVTCPSocket.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -93,7 +93,7 @@ void *LVTCPLoop(void *arg){
9393
}
9494
read(new_socket, buffer, 1024);
9595

96-
printf("RECEIVED: %s\n",buffer);
96+
// printf("RECEIVED: %s\n",buffer);
9797

9898
// Do things
9999
if(!strncmp(buffer, "power off", MAX_COMMAND_SIZE)){

‎middleware/src/LVTelemetry_Loop.cpp

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
extern "C"
1717
{
1818
#include "lv_iox.h"
19+
#include "imu.h"
1920
}
2021

2122

@@ -77,21 +78,24 @@ void *LVTelemetryLoop(void *arg)
7778

7879
// POSITION
7980
Value pos;
80-
pos.SetFloat(data->motion->pos);
81-
81+
//pos.SetFloat(getPosX());
82+
pos.SetFloat(0);
83+
8284
// RETRO
8385
Value retro;
8486
retro.SetInt(data->motion->retroCount);
8587

8688
// VELOCITY - Change "X" to "Y" if need be
8789
Value vel;
88-
vel.SetFloat(data->motion->vel);
90+
//vel.SetFloat(getVelX());
91+
vel.SetFloat(0);
8992

9093
Value lstRet;
9194
lstRet.SetUint64(data->timers->lastRetro);
9295
// ACCELERATION - Change "X" to "Y" if need be
9396
Value accel;
94-
accel.SetFloat(data->motion->accel);
97+
accel.SetFloat(-1 * getAccelX());
98+
//accel.SetFloat(0);
9599

96100
// PRESSURE VESSEL PRESSURE
97101
Value pressureV;

‎middleware/src/data_dump.cpp

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ void currentDateTime(char *buf) {
3636

3737
/* Thread Loop */
3838
void *DataLoop(void *arg){
39-
39+
4040
(void) arg;
4141

4242
FILE *fp;
@@ -45,16 +45,18 @@ void *DataLoop(void *arg){
4545
char timestamp[80];
4646
currentDateTime(timestamp);
4747

48-
sprintf(dir, "../data_logs/%s.csv", timestamp);
48+
sprintf(dir, "../../data_logs/LOG.csv", timestamp);
4949

5050
fp = fopen (dir,"w");
5151
if (fp == NULL){
5252
printf("Unable to open file\n");
5353
return NULL;
5454
}
55+
5556

56-
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");
5757

58+
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");
59+
5860
while(1){
5961

6062
// FLAGS

0 commit comments

Comments
 (0)