Skip to content

Commit 76709f9

Browse files
author
Ezra Boley
committed
Working on getting the state machine finished
1 parent d6d86b4 commit 76709f9

File tree

17 files changed

+140
-68
lines changed

17 files changed

+140
-68
lines changed

‎Makefile‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,9 @@ ifdef DEBUG
1919
DEBUG_MODE := DEBUG_RETRO DEBUG_RMS DEBUG_BMS DEBUG_PRES
2020
endif
2121

22+
ifdef NF
23+
NO_FAULT := NO_FAULT
24+
endif
2225
ifdef LOCAL
2326
NOI2C := NOI2C
2427
endif
@@ -37,7 +40,7 @@ GCC := $(BEAGLE)gcc
3740
GPP := $(BEAGLE)g++
3841
IFLAGS := $(addprefix -I,$(INCLUDE_DIRS))
3942
WFLAGS := -Wall -Wno-deprecated -Wextra -Wno-type-limits -fdiagnostics-color
40-
CFLAGS := -std=gnu11 $(addprefix -D,$(USE_VCAN)) $(addprefix -D, $(DEBUG_MODE)) $(addprefix -D, $(NOI2C))
43+
CFLAGS := -std=gnu11 $(addprefix -D,$(USE_VCAN)) $(addprefix -D, $(DEBUG_MODE)) $(addprefix -D, $(NOI2C)) $(addprefix -D, $(NF))
4144
CPFLAGS := -std=c++11
4245
LDFLAGS := -Llib
4346
LDLIBS := -lm -lpthread

‎embedded/app/main/badgerloop_HV.cpp‎

Lines changed: 26 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
#include <stdio.h>
33
#include <stdlib.h>
44
#include <unistd.h>
5-
5+
#include <signal.h>
66
#include "HVTelemetry_Loop.h"
77
#include "HVTCPSocket.h"
88
#include "HV_Telem_Recv.h"
@@ -13,7 +13,8 @@
1313
extern "C"
1414
{
1515
#include <signal.h>
16-
#include "motor.h"
16+
#include <rms.h>
17+
#include "motor.h"
1718
#include "hv_iox.h"
1819
#include "motor.h"
1920
#include "proc_iox.h"
@@ -22,6 +23,20 @@ extern "C"
2223
#include "state_machine.h"
2324
#include "NCD9830DBR2G.h"
2425
}
26+
extern bool noTorqueMode;
27+
extern bool motorIsEnabled;
28+
void emergQuitter(int sig, siginfo_t* inf, void *nul) {
29+
printf("shutdown\n");
30+
motorIsEnabled = false;
31+
noTorqueMode = false;
32+
usleep(10000);
33+
rmsCmdNoTorque();
34+
sleep(1);
35+
rmsDischarge();
36+
sleep(1);
37+
rmsInvDis();
38+
exit(0);
39+
}
2540

2641
int init() {
2742
/* Init Data struct */
@@ -31,7 +46,7 @@ int init() {
3146
SetupCANDevices();
3247
initProcIox(true);
3348
initHVIox(true);
34-
initMotor();
49+
/* initMotor(); */
3550
initPressureSensors();
3651
/* Allocate needed memory for state machine and create graph */
3752
buildStateMachine();
@@ -40,7 +55,10 @@ int init() {
4055
SetupHVTelemetry((char *) DASHBOARD_IP, DASHBOARD_PORT);
4156
SetupHVTCPServer();
4257
SetupHVTelemRecv();
43-
58+
59+
struct sigaction sig;
60+
sig.sa_sigaction = emergQuitter;
61+
sigaction(SIGINT, &sig, NULL);
4462
/* Start 'black box' data saving */
4563
/* SetupDataDump();*/
4664

@@ -78,7 +96,10 @@ int main() {
7896
signalLV((char *)"secBrakeOff");
7997
data->flags->brakeInit = false;
8098
}
81-
99+
if (data->flags->clrMotionData) {
100+
signalLV((char *) "clrMotion");
101+
data->flags->clrMotionData = false;
102+
}
82103

83104
usleep(10000);
84105

‎embedded/app/src/motor.c‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -103,9 +103,8 @@ int startMotor() {
103103
rmsInvDis();
104104
/* idleMotor();*/
105105
sleep(3); /* Not ideal, but unless we have a way to check status this stays */
106-
setMotorIsOn(true);
107-
usleep(100000);
108106
rmsInvEnNoTorque();
107+
setMotorIsOn(true);
109108
return 0;
110109
}
111110

@@ -141,9 +140,10 @@ static void *motorHeartbeatLoop(void *unusedParam) {
141140
sem_wait(&hbSem);
142141
if (getMotorIsOn()) {
143142
printf("MOTOR IS ON\n");
144-
if (rmsSendHbMsg(getTorque()) != 0) {
145-
fprintf(stderr, "Failed to send heartbeat\n");
146-
}
143+
rmsInvEn();
144+
/* if (rmsSendHbMsg(getTorque()) != 0) {*/
145+
/* fprintf(stderr, "Failed to send heartbeat\n");*/
146+
/* }*/
147147
} else {
148148
if (rmsIdleHb() != 0) {
149149
fprintf(stderr, "Failed to send idle heartbeat\n");

‎embedded/app/src/nav.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -150,7 +150,7 @@ void filterMotion(int filterType) {
150150
data->motion->accel = accel;
151151
imuDirFn[CURR_DIR].setPos(0);
152152
}
153-
153+
void resetNav();
154154
void resetNav() {
155155
data->motion->pos = 0;
156156
data->motion->vel = 0;

‎embedded/app/src/state_machine.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -301,7 +301,7 @@ void buildStateMachine(void) {
301301
stateMachine.allStates[i] = malloc(sizeof(state_t));
302302
}
303303

304-
initState(stateMachine.allStates[0], IDLE_NAME, idleAction, 4);
304+
initState(stateMachine.allStates[0], IDLE_NAME, idleAction, 3);
305305
initState(stateMachine.allStates[1], PUMPDOWN_NAME, pumpdownAction, 2);
306306
initState(stateMachine.allStates[2], PROPULSION_NAME, propulsionAction, 2);
307307
initState(stateMachine.allStates[3], BRAKING_NAME, brakingAction, 3);

‎embedded/app/src/states.c‎

Lines changed: 39 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -18,10 +18,16 @@
1818
#include "data.h"
1919
#include "states.h"
2020
#include "hv_iox.h"
21-
21+
#include <math.h>
2222
#include "bms_fault_checking.h"
2323
#include "rms_fault_checking.h"
2424
#include "pressure_fault_checking.h"
25+
#define NO_FAULT
26+
#define LV_BATT_SOC_CALC(x) (pow(-1.1142 * (x), 6) + \
27+
pow(78.334 * (x), 5) - \
28+
pow(2280.5 * (x), 4) + \
29+
pow(35181.0 * (x), 3) - \
30+
pow(303240.0 *(x),2) - (1000000.0 * (x)))
2531

2632
/* Imports/Externs */
2733

@@ -50,42 +56,46 @@ static bool checkStopped(void) {
5056

5157
stateTransition_t * idleAction() {
5258
static firstCycle = true;
53-
59+
static displayBatt = 0;
5460
if (firstCycle) {
5561
data->flags->brakeInit = true;
5662
/* Does this! */
5763
/* brakePrimaryUnactuate();*/
5864
/* brakeSecondaryUnactuate();*/
5965
firstCycle = false;
6066
}
61-
67+
68+
if (displayBatt >= 100) {
69+
printf("LV SOC: %f\n", LV_BATT_SOC_CALC(getLVBattVoltage()));
70+
displayBatt = 0;
71+
} else
72+
displayBatt += 1;
6273
data->state = 1;
6374

6475
if (data->flags->emergencyBrake) {
6576
return findTransition(stateMachine.currState ,RUN_FAULT_NAME);
6677
}
6778

6879
// CHECK PRESSURE
69-
/* if(!checkPrerunPressures()){*/
70-
/* return stateMachine.currState->fault;*/
71-
/* }*/
72-
73-
// CHECK STOPPED (MOTION)
74-
/* if(!checkStopped()){*/
75-
/* return stateMachine.currState->fault;*/
76-
/* }*/
80+
#ifndef NO_FAULT
81+
if(!checkPrerunPressures()){
82+
return stateMachine.currState->fault;
83+
}
84+
#endif
7785

7886
// TODO check LV Power
79-
// TODO check LV Temp
8087

81-
/* if(!checkPrerunBattery()){*/
82-
/* return stateMachine.currState->fault;*/
83-
/* }*/
84-
85-
/* if(!checkPrerunRMS()){*/
86-
/* return stateMachine.currState->fault;*/
87-
/* }*/
88+
89+
// TODO check LV Temp
90+
#ifndef NO_FAULT
91+
if(!checkPrerunBattery()){
92+
return stateMachine.currState->fault;
93+
}
8894

95+
if(!checkPrerunRMS()){
96+
return stateMachine.currState->fault;
97+
}
98+
#endif
8999
// TRANSITION CRITERIA
90100
if(data->flags->readyPump){
91101
return findTransition(stateMachine.currState, PUMPDOWN_NAME);
@@ -102,7 +112,7 @@ stateTransition_t * pumpdownAction() {
102112
if (data->flags->emergencyBrake) {
103113
return findTransition(stateMachine.currState ,RUN_FAULT_NAME);
104114
}
105-
115+
#ifndef NO_FAULT
106116
if (!getIMDStatus()) {
107117
return stateMachine.currState->fault;
108118
}
@@ -112,17 +122,11 @@ stateTransition_t * pumpdownAction() {
112122
return stateMachine.currState->fault;
113123
}
114124

115-
116125
// CHECK PRESSURE
117-
/* if(!checkPrerunPressures()){*/
118-
/* return stateMachine.currState->fault;*/
119-
/* }*/
120-
121-
// CHECK STOPPED (MOTION)
122-
if(!checkStopped()){
126+
if(!checkPrerunPressures()){
123127
return stateMachine.currState->fault;
124128
}
125-
129+
#endif
126130
// TODO check LV Power
127131
// TODO check LV Temp
128132

@@ -144,9 +148,10 @@ stateTransition_t * pumpdownAction() {
144148

145149
stateTransition_t * propulsionAction() {
146150
static bool isInit = false;
151+
147152
data->state = 3;
148153
if (!isInit) {
149-
154+
data->flags->clrMotionData = true;
150155
data->timers->startTime = getuSTimestamp();
151156
isInit = true;
152157
}
@@ -188,7 +193,7 @@ stateTransition_t * propulsionAction() {
188193
return findTransition(stateMachine.currState, BRAKING_NAME);
189194
}
190195

191-
if (data->motion->retroCount >= 3) {
196+
if (data->motion->retroCount >= 3 && data->flags->readyToBrake) {
192197
printf("here3\n");
193198
return findTransition(stateMachine.currState, BRAKING_NAME);
194199
}
@@ -211,6 +216,7 @@ stateTransition_t * brakingAction() {
211216
if (data->flags->emergencyBrake) {
212217
return findTransition(stateMachine.currState ,RUN_FAULT_NAME);
213218
}
219+
#ifndef NO_FAULT
214220
if (!getIMDStatus()) {
215221
return stateMachine.currState->fault;
216222
}
@@ -236,15 +242,15 @@ stateTransition_t * brakingAction() {
236242
if(!checkBrakingRMS()){
237243
return stateMachine.currState->fault;
238244
}
239-
245+
#endif
240246
if ((getuSTimestamp() - data->timers->startTime) > MAX_RUN_TIME){
241247
return stateMachine.currState->fault;
242248
}
243249

244250
// CHECK TRANSITION CRITERIA
245-
if(checkStopped()){
251+
/* if(checkStopped()){
246252
return findTransition(stateMachine.currState, STOPPED_NAME);
247-
}
253+
}*/
248254

249255

250256
return NULL;

‎embedded/data/include/data.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@ typedef struct flags_t {
3737
int readyPump;
3838
int pumpDown;
3939
int readyCommand;
40+
bool readyToBrake;
4041
int propulse;
4142
int emergencyBrake;
4243
int shouldStop;
@@ -48,6 +49,7 @@ typedef struct flags_t {
4849
bool brakeSecAct;
4950
bool brakePrimRetr;
5051
bool brakeSecRetr;
52+
bool clrMotionData;
5153
} flags_t;
5254

5355

‎embedded/examples/brakingTest.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -16,10 +16,10 @@ void showBrakingInfo() {
1616
int i = 0;
1717
pressure_t *p = data->pressure;
1818
//showPressures();
19-
fprintf(stderr,"%f,%f,%f,%f,%f,%f\n", p->primTank, p->primLine, p->primAct,
20-
p->secTank, p->secLine, p->secAct);
21-
printf("%f,%f,%f,%f,%f,%f\n", p->primTank, p->primLine, p->primAct,
22-
p->secTank, p->secLine, p->secAct);
19+
fprintf(stderr,"%f,%f,%f,%f,%f,%f,%f\n", p->primTank, p->primLine, p->primAct,
20+
p->secTank, p->secLine, p->secAct, p->pv);
21+
printf("%f,%f,%f,%f,%f,%f,%f\n", p->primTank, p->primLine, p->primAct,
22+
p->secTank, p->secLine, p->secAct,p->pv);
2323
}
2424

2525

@@ -52,8 +52,8 @@ int main(int argc, char *argv[]) {
5252
}
5353
}
5454
}
55-
fprintf(stderr, "primTank,primLine,primAct,secTank,secLine,secAct\n");
56-
printf("primTank,primLine,primAct,secTank,secLine,secAct\n");
55+
fprintf(stderr, "primTank,primLine,primAct,secTank,secLine,secAct,pv\n");
56+
printf("primTank,primLine,primAct,secTank,secLine,secAct,pv\n");
5757
FOREVER {
5858
showBrakingInfo();
5959
usleep(100000);

‎embedded/peripherals/src/batt.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66
#define CURR_SENSOR CHANNEL_7
77

88
#define VOLT_SCALE(x) (((((double) x) / 255.0) * 5.0000 * 2.942))
9-
#define CURR_SCALE(x) (((((double) x) / 255.0) * 5.0000))
9+
#define CURR_SCALE(x) ((((((double) x) / 255.0) * 5.0000) - 2.5 / 2.5) * 50)
1010
double getLVBattVoltage();
1111
double getLVCurrent();
1212
double getLVBattVoltage() {

‎embedded/peripherals/src/braking.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -87,7 +87,7 @@ static double avgDouble(double *arr, int size) {
8787
int brake() {
8888
brakePrimaryActuate();
8989
usleep(500000);
90-
if (!limSwitchGet(PRIM_LIM_SWITCH))
90+
if (limSwitchGet(PRIM_LIM_SWITCH))
9191
brakeSecondaryActuate();
9292
return 0;
9393
}

0 commit comments

Comments
 (0)