Skip to content

Commit

Permalink
Trace the motor current and voltage
Browse files Browse the repository at this point in the history
  • Loading branch information
@henrychoi
henrychoi committed Sep 19, 2020
1 parent c923b83 commit 65bc1a4
Show file tree
Hide file tree
Showing 2 changed files with 52 additions and 31 deletions.
10 changes: 5 additions & 5 deletions f446-ihm08-castle/Src/mc_tasks.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,6 @@ struct FastTrace {
uint8_t bitmap;
uint8_t HallState;
int16_t ElAngle; // 4
int16_t ElSpeed;
ab_t Iab; // 8
qd_t Iqd; // 12
//qd_t Vqd;
Expand All @@ -107,7 +106,6 @@ static void FastTrace_write(struct FastTrace* trace) {
trace->bitmap = (HALL_M1.SensorIsReliable << 1) | HALL_M1.Direction;
trace->HallState = HALL_M1.HallState;
trace->ElAngle = HALL_M1.MeasuredElAngle;
trace->ElSpeed = HALL_M1.AvrElSpeedDpp;
//trace->error = hFOCreturn;
trace->Iab = FOCVars[M1].Iab;
trace->Iqd = FOCVars[M1].Iqd;
Expand All @@ -119,8 +117,10 @@ struct MedTrace {
uint8_t pad;
uint8_t seq;
struct FastTrace _Super;
int16_t speed, target; // 20
qd_t Iqdref; // 24
int16_t ElSpeed;
//int16_t speed
int16_t target;
qd_t Iqdref;
};
#if 0
// 256 @ 1000 Hz, 5 RPS should cover at least 1 period
Expand Down Expand Up @@ -503,7 +503,7 @@ __weak void TSK_MediumFrequencyTaskM1(void)
trace.seq = ++sSN;
FastTrace_write(&trace._Super);
trace._Super.bitmap |= StateM1 << 2;
trace.speed = SPD_GetAvrgMecSpeedUnit(&HALL_M1._Super);
trace.ElSpeed = HALL_M1.AvrElSpeedDpp;
trace.target = pSTC[M1]->TargetFinal;
trace.Iqdref = FOCVars[M1].Iqdref;
static uint8_t cobsBuffer[sizeof(trace) + 2] = {0};
Expand Down
73 changes: 47 additions & 26 deletions scripts/med_trace.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
def raw2int16(n): return n if n < 0x8000 else n - 0x10000
def int32(n): return n if n < 0x80000000 else n - 0x100000000

sMedTraceFormat = "{},"*15 + "{}\n"
sMedTraceFormat = "{},"*14 + "{}\n"

class MedTrace:
def __init__(self, outf, Nsample):
Expand All @@ -28,7 +28,7 @@ def __init__(self, outf, Nsample):

# @return void
def parse(self, arr):
if len(arr) < 27:
if len(arr) < 25:
sys.stderr.write("Unexpected packet {}".format(str(binascii.hexlify(arr))))
return

Expand All @@ -37,15 +37,14 @@ def parse(self, arr):
bitmap = arr[idx]; idx += 1
hallState = arr[idx]; idx += 1
elAngle = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2
elSpeed = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2
Ia = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2
Ib = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2
Iq = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2
Id = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2
Valph = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2
Vbeta = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2
meSpeed = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2
meTarget = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2
elSpeed = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2
target = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2
Iqref = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2
Idref = raw2int16(arr[idx] + arr[idx+1] * 256); idx += 2

Expand All @@ -57,9 +56,9 @@ def parse(self, arr):
sys.stderr.write("-{}".format(seq))
self.seq = seq
self.outf.write(sMedTraceFormat.format(
seq, state, direction, hallState, elAngle, elSpeed,
seq, state, direction, hallState, elAngle,
Ia, Ib, Iq, Id, Valph, Vbeta,
meSpeed, meTarget, Iqref, Idref))
elSpeed, target, Iqref, Idref))
self.row += 1

# @param s COBS encoded byte array
Expand Down Expand Up @@ -95,8 +94,8 @@ def main(argv):
tty.setcbreak(sys.stdin.fileno()) # make the stdin nonblocking
#ser = open(dev)
#ser.reset_input_buffer()
with open(outfn, 'w', Nsample) as outf, serial.Serial(dev, 3000000, timeout=10) as ser:
outf.write('seq, state, direction, hallState, elAngle, elSpeed, Ia, Ib, Iq, Id, Valph, Vbeta, meSpeed, meTarget, Iqref, Idref\n')
with open(outfn, 'w') as outf, serial.Serial(dev, 3000000, timeout=10) as ser:
outf.write('seq, state, direction, hallState, elAngle, Ia, Ib, Iq, Id, Valph, Vbeta, elSpeed, target, Iqref, Idref\n')
med = MedTrace(outf, Nsample)
while med.process(ser.read(256)): # try to batch, for efficiency
if select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []): # want to bail?
Expand All @@ -107,39 +106,61 @@ def main(argv):

# Plot
# read the data file back again; faster than building up the data file 1 row at a time
seq, state, direction, hallState, elAngle, elSpeed, \
Ia, Ib, Iq, Id, Valph, \
Vbeta, meSpeed, meTarget, Iqref, Idref = \
np.loadtxt(outfn, dtype=np.int16, unpack=True, max_rows=Nsample, delimiter = ',')
seq, state, direction, hallState, elAngle, Ia, Ib, Iq, Id, Valph, \
Vbeta, elSpeed, target, Iqref, Idref = \
np.loadtxt(outfn, dtype=np.int16, unpack=True, delimiter = ',',
skiprows=1, max_rows=Nsample)

# Convert to easy-to-see unit
eAngle = elAngle * 180.0 / 0x10000
# Convert to easy-to-read unit
eAngle = elAngle * 180.0 / 0x8000
eSpeed = elSpeed * 0.1 # * 180 / np.pi
mSpeed = meSpeed * 0.1 # * 180 / np.pi
#mSpeed = meSpeed * 0.1 # * 180 / np.pi
target = target * 0.1
Vdd = 3.3; Rshunt = 0.01; Gaop = 5.18
Imax = Vdd / (2 * Rshunt * Gaop)
Vmax = 12.0
Ia = Ia * Imax / 0x8000; Ib = Ib * Imax / 0x8000
Id = Id * Imax / 0x8000; Iq = Iq * Imax / 0x8000
Idref = Idref * Imax / 0x8000; Iqref = Iqref * Imax / 0x8000
Valph = Valph * Vmax / 0x8000; Vbeta = Vbeta * Vmax / 0x8000

t = 0.001 * np.linspace(0, len(seq), num=len(seq))

fig = plt.figure(figsize=(12, 12)) # width, height size
Nrow = 3
yy1 = plt.subplot(Nrow,1,1)
yy1.plot(t, seq, ls=':', label='SN')
yy1.plot(t, seq, '.', marker='.', markersize=1, label='SN')
yy1.plot(t, state, label='MC state')
yy1.plot(t, direction, ls='-.', label='direction')
#yy1.scatter(t, state, label='state')
yy1.plot(t, eAngle, label='E ∠')
yy1.legend(bbox_to_anchor=(0, 1.02, 1., .102), #left, bottom, width, height
ncol=4, mode="expand", borderaxespad=0.5)
#yy1.set(ylabel='Motor State')
yy1.plot(t, eAngle, label='E ∠ [°]')
yy1.legend(bbox_to_anchor=(0, 1.02, 1., .102), ncol=5, mode="expand", borderaxespad=0.5)
yy1.set(ylabel='[°]')

yy2 = yy1.twinx()
yy2.plot(t, direction, ls='-.', label='direction')
yy2.scatter(t, hallState, label='Hall')
yy2.plot(t, eSpeed, label='Espeed')
yy2.plot(t, mSpeed, label='Mspeed')
#yy2.plot(t, hallState, label='Hall')
yy2.plot(t, eSpeed, label='Espeed [rad/s]')
yy2.plot(t, target, label='target [rad/s]')
yy2.set(ylabel='[1/s]')
yy2.legend()

#yy3 = yy1.twinx()

#yy1 = plt.subplot(Nrow,1,2)
yy1 = plt.subplot(Nrow,1,2)
yy1.plot(t, Ia, ls=':', label='Ia')
yy1.plot(t, Ib, ls=':', label='Ib')
yy1.plot(t, Id, label='Id')
yy1.plot(t, Iq, label='Iq')
yy1.plot(t, Idref, ls='-.', label='Id*')
yy1.plot(t, Iqref, ls='-.', label='Iq*')
yy1.set(ylabel='[A]')
yy1.legend(bbox_to_anchor=(0, 1.02, 1., .102), ncol=6, mode="expand", borderaxespad=0.5)

yy2 = yy1.twinx()
yy2.plot(t, Valph, ls='--', label=r'$V_\alpha$')
yy2.plot(t, Vbeta, ls='--', label=r'$V_\beta$')
yy2.set(ylabel='[V]')
yy2.legend()

plt.tight_layout()
plt.show()
Expand Down

0 comments on commit 65bc1a4

Please sign in to comment.