void zero_amploc(void) // set zero current setpoint from ADC reading from a10_x a10_z a10_y, write to EEPROM
{
static uint8_t z;
adc_cal[11] = adc_cal[12] = adc_cal[13] = adc_cal[14] = ADC_NULL; // reset offsets to zero (ADC_NULL)
// read adc data for zero setpoint
ADC_read(); // a10_x=adc_[11], a10_z=adc_cal[12], a10_y=adc_cal[13]
adc_cal[11] = (uint8_t) (ADC_NULL + (int) ((int) a10_x - (int) AMP10_ZERO));
adc_cal[12] = (uint8_t) (ADC_NULL + (int) ((int) a10_z - (int) AMP10_ZERO));
adc_cal[13] = (uint8_t) (ADC_NULL + (int) ((int) a10_y - (int) AMP10_ZERO));
if (checktime_eep(EEP_UPDATE, FALSE)) {
term_time();
putrs2USART(zero0);
sprintf(bootstr2, "\n\r Zero offset %i, X current %i, AMP10_ZERO %i", (int) adc_cal[11], a10_x, AMP10_ZERO);
puts2USART(bootstr2);
sprintf(bootstr2, "\n\r Zero offset %i, Y current %i, AMP10_ZERO %i", (int) adc_cal[12], a10_z, AMP10_ZERO);
puts2USART(bootstr2);
sprintf(bootstr2, "\n\r Zero offset %i, Z current %i, AMP10_ZERO %i", (int) adc_cal[13], a10_y, AMP10_ZERO);
puts2USART(bootstr2);
write_data_eeprom(adc_cal[z], ADC_SLOTS, z, 8);
checktime_eep(EEP_UPDATE, TRUE);
putrs2USART(zero1);
}
}
void G1ParScanThreadState::print_termination_stats(outputStream* const st) const {
const double elapsed_ms = elapsed_time() * 1000.0;
const double s_roots_ms = strong_roots_time() * 1000.0;
const double term_ms = term_time() * 1000.0;
size_t alloc_buffer_waste = 0;
size_t undo_waste = 0;
_plab_allocator->waste(alloc_buffer_waste, undo_waste);
st->print_cr("%3u %9.2f %9.2f %6.2f "
"%9.2f %6.2f " SIZE_FORMAT_W(8) " "
SIZE_FORMAT_W(7) " " SIZE_FORMAT_W(7) " " SIZE_FORMAT_W(7),
_worker_id, elapsed_ms, s_roots_ms, s_roots_ms * 100 / elapsed_ms,
term_ms, term_ms * 100 / elapsed_ms, term_attempts(),
(alloc_buffer_waste + undo_waste) * HeapWordSize / K,
alloc_buffer_waste * HeapWordSize / K,
undo_waste * HeapWordSize / K);
}
void ADC_read(void) // update all voltage/current readings and set load current in 'currentload' variable
{ // ADC is opened and config'd in main
static uint16_t i, z, change = 0; // used for fast and slow sample loops >256
static int32_t a10_x_t, a10_y_t, a10_z_t;
ClrWdt(); // reset the WDT timer
ADC_zero();
SetChanADC(ADC_CH0); // A0 system
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
vbatol_t = Vin * (ADC0_MV - ADC_NULL + adc_cal[0]); // voltage correction factor
vbatol_t = vbatol_t / 100;
ADC_zero();
SetChanADC(ADC_CH1); // A1 motor
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
solar_t = Vin * (ADC1_MV - ADC_NULL + adc_cal[1]);
solar_t = solar_t / 100;
ADC_zero();
SetChanADC(ADC_CH2); // A2 current_x
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_S; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_S;
a10_x = Vin; // raw ADC value
Vin = Vin - (AMP10_ZERO - ADC_NULL + adc_cal[11]); // set zero NULL0 point
a10_x_t = (long) Vin * (long) (ADC2_MV - ADC_NULL + adc_cal[2]);
if (ABSL(a10_x_t) < AMPZ)
a10_x_t = 0; // zero bit noise
a10_x_t = (long) ((float) a10_x_t / (float) AMP10_SEN_H);
a10_x_t = (long) lp_filter((float) a10_x_t, LP_CURRENT_X, FALSE); // use digital filter
if ((a10_x == NULL0) || (a10_x_t < 0.0)) a10_x_t = 0; // if sensor disconnected read zero
ADC_zero();
SetChanADC(ADC_CH11); // A11 current_y, switch from 3 so we can use external VREF
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_S; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_S; //
a10_y = Vin; // raw ADC value
Vin = Vin - (AMP10_ZERO - ADC_NULL + adc_cal[12]);
a10_y_t = (long) Vin * (long) (ADC3_MV - ADC_NULL + adc_cal[3]);
if (ABSL(a10_y_t) < AMPZ)
a10_y_t = 0; // zero bit noise
a10_y_t = (long) ((float) a10_y_t) / ((float) AMP10_SEN_L);
a10_y_t = (long) lp_filter((float) a10_y_t, LP_CURRENT_Y, FALSE); // use digital filter
if ((a10_y == NULL0) || (a10_y_t < 0.0)) a10_y_t = 0; // if sensor disconnected (zero) read zero current
ADC_zero();
SetChanADC(ADC_CH4); // A5 current_z
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_S; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_S;
a10_z = Vin;
Vin = Vin - (AMP10_ZERO - ADC_NULL + adc_cal[13]); // set zero NULL0 point
a10_z_t = (long) Vin * (long) (ADC4_MV - ADC_NULL + adc_cal[4]);
if (ABSL(a10_z_t) < AMPZ)
a10_z_t = 0; // zero bit noise
a10_z_t = (long) ((float) a10_z_t / (float) AMP10_SEN_L);
a10_z_t = (long) lp_filter((float) a10_z_t, LP_CURRENT_Z, FALSE);
if ((a10_z == NULL0) || (a10_z_t < 0.0)) a10_z_t = 0; // if sensor disconnected read zero
ADC_zero();
SetChanADC(ADC_CH5); // F0 pot x
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
rawp[0] = Vin;
ADC_zero();
SetChanADC(ADC_CH6); // F1 pot y
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
rawp[1] = Vin;
ADC_zero();
SetChanADC(ADC_CH7); // F2 pot z
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
rawp[2] = Vin;
ADC_zero();
SetChanADC(ADC_CH8); // F0 pot max
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
rawa[0] = Vin;
ADC_zero();
SetChanADC(ADC_CH9); // F1 pot max
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
rawa[1] = Vin;
ADC_zero();
SetChanADC(ADC_CH10); // F2 pot max
Delay10TCYx(ADC_CHAN_DELAY);
Vin = 0;
for (i = 0; i < ADC_SAMP_F; i++) {
ConvertADC();
while (BusyADC());
Vin += (uint16_t) ReadADC();
}
Vin /= ADC_SAMP_F;
rawa[2] = Vin;
R.motorvoltage = solar_t;
R.systemvoltage = vbatol_t;
if (SYSTEM_STABLE) {
s_crit(HL);
R.current_z = a10_z_t;
R.current_y = a10_y_t;
R.current_x = a10_x_t;
R.pos_x = rawp[XAXIS];
R.pos_y = rawp[YAXIS];
R.pos_z = rawp[ZAXIS];
R.max_x = rawa[XAXIS];
R.max_y = rawa[YAXIS];
R.max_z = rawa[ZAXIS];
e_crit();
for (z = 0; z < MAX_POT; z++) {
motordata[z].pot.pos_actual = rawp[z];
if (ABSI(motordata[z].pot.pos_actual - motordata[z].pot.pos_actual_prev) > motordata[z].pot.pos_change) {
motordata[z].pot.pos_change = ABSI(motordata[z].pot.pos_actual - motordata[z].pot.pos_actual_prev);
}
if (motordata[z].active && (motordata[z].pot.pos_change > motordata[z].pot.limit_change)) {
if (mode.change) {
term_time();
sprintf(bootstr2, " Pot %i Change too high %i\r\n", z, motordata[z].pot.pos_change);
puts2USART(bootstr2);
motordata[z].pot.pos_change = motordata[z].pot.limit_change; // after one message stop and set it to the limit.
} else {
motordata[z].pot.pos_actual = rawp[z]; // set to current pot reading
motordata[z].pot.pos_actual_prev = rawp[z];
motordata[z].pot.pos_change = 0; // pot position change mag
}
}
// Check for POT Dead-Spot readings
if ((motordata[z].pot.pos_change >= POT_MAX_CHANGE) && !mode.qei) motordata[z].pot.cal_failed = TRUE;
if (!motordata[z].active) motordata[z].pot.cal_failed = FALSE; // don't fail inactive motors
motordata[z].pot.pos_actual_prev = motordata[z].pot.pos_actual;
if (motordata[z].pot.pos_actual > motordata[z].pot.high) motordata[z].pot.high = motordata[z].pot.pos_actual; // set adc limits of values X,Y,Z
if (motordata[z].pot.pos_actual < motordata[z].pot.low) motordata[z].pot.low = motordata[z].pot.pos_actual;
if (mode.operate == VIISION_MS) { // use the preset o-100 resistance limits for scaling
motordata[z].pot.high = VIISION_MS_RES_HIGH;
motordata[z].pot.low = VIISION_MS_RES_LOW;
}
motordata[z].pot.offset = motordata[z].pot.low;
motordata[z].pot.span = motordata[z].pot.high - motordata[z].pot.low;
if (motordata[z].pot.span < 0) motordata[z].pot.span = 0;
motordata[z].pot.scale_out = SCALED_FLOAT / motordata[z].pot.span;
motordata[z].pot.scale_in = motordata[z].pot.span / SCALED_FLOAT;
motordata[z].pot.scaled_actual = (int) ((float) (motordata[z].pot.pos_actual - motordata[z].pot.offset) * motordata[z].pot.scale_out);
if (motordata[z].pot.scaled_actual > SCALED) motordata[z].pot.scaled_actual = SCALED;
motordata[z].pot.pos_set = (int) (((float) motordata[z].pot.scaled_set * motordata[z].pot.scale_in) + motordata[z].pot.offset);
}
if (mode.emo) {
term_time();
sprintf(bootstr2, " EMO flag tripped, possible short circuit in the assy wiring.\r\n");
puts2USART(bootstr2);
term_time();
sprintf(bootstr2, " EMO DUMP Current %li, %li, %li : Position %li, %li, %li\r\n", emodump.emo[0], emodump.emo[1], emodump.emo[2], emodump.emo[3], emodump.emo[4], emodump.emo[5]);
puts2USART(bootstr2);
voice2_ticks(40);
while (TRUE) {
emo_display();
buzzer_ticks(200);
ClrWdt(); // reset the WDT timer
}
}
}
ADC_zero(); // ground ADC input
ClrWdt(); // reset the WDT timer
}
/* assembly type data configuration */
void init_motordata(uint8_t part)
{
static uint8_t z, old_part = 255;
if (part == old_part) return;
old_part = part;
putrs2USART("\r\n");
term_time();
sprintf(bootstr2, "\x1b[7m Init Motor Data. Part %i \x1b[0m\r\n", part);
puts2USART(bootstr2);
// setup motor configs
mode.v24 = FALSE;
mode.free = TRUE;
mode.operate = part;
mode.display = default_display; // display screen function pointer
mode.on_off_only = FALSE;
mode.info_only = FALSE;
mode.qei = FALSE;
mode.change = FALSE;
ADC_read();
s_crit(HL); // stop ISR from updating
emotor_power_off(); // turn off the power first
for (z = 0; z < MAX_MOTOR; z++) { // default motor setups
motordata[z].type = part;
motordata[z].run = TRUE;
motordata[z].cw = TRUE;
motordata[z].axis = z;
motordata[z].hunt_count = 0;
motordata[z].free = TRUE;
motordata[z].slow = FALSE;
motordata[z].slow_only = FALSE;
motordata[z].active = TRUE;
motordata[z].reversed = FALSE;
motordata[z].v24 = FALSE;
motordata[z].cal_pos = 500;
motordata[z].pot.pos_set = 512; // mid range
motordata[z].pot.pos_actual = rawp[z]; // set to current pot reading
motordata[z].pot.scaled_set = 500; // mid range
motordata[z].pot.pos_actual_prev = rawp[z];
motordata[z].pot.movement = STOP; // no motion
motordata[z].pot.pos_change = 0; // pot position change mag
motordata[z].pot.low = 1023; // init limit detection values
motordata[z].pot.high = 0; // ditto
motordata[z].pot.cal_low = FALSE;
motordata[z].pot.cal_high = FALSE;
motordata[z].pot.cal_failed = TRUE;
motordata[z].pot.cal_warn = FALSE;
motordata[z].pot.limit_change = POT_MAX_CHANGE;
motordata[z].pot.limit_span = POT_MIN_SPAN;
motordata[z].pot.limit_offset = POT_M_OFFSET;
motordata[z].pot.limit_offset_l = POT_L_OFFSET;
motordata[z].pot.limit_offset_h = POT_H_OFFSET;
}
if (part == E220E500_E) {
term_time();
mode.display = e220_qei_display;
putrs2USART("\x1b[7m Init E220E500 QEI MOTOR. \x1b[0m\r\n");
for (z = 0; z < MAX_MOTOR; z++) {
motordata[z].slow = FALSE;
motordata[z].slow_only = FALSE;
motordata[z].active = FALSE;
motordata[z].reversed = FALSE;
motordata[z].v24 = FALSE;
}
motordata[0].active = TRUE; // X motor for assy position
mode.qei = TRUE;
qei1.c = 0; // set motor and set knob to zero counts
knob2.c = 0;
V.qei_counts = 0;
V.pwm4int_count = 0;
}
if (part == V810_MS || part == EV810_MS) {
term_time();
mode.display = v810_ms_display;
putrs2USART("\x1b[7m Init VISTA MASS SLIT. \x1b[0m\r\n");
for (z = 0; z < MAX_MOTOR; z++) {
motordata[z].slow = FALSE;
motordata[z].slow_only = FALSE;
motordata[z].active = FALSE;
motordata[z].reversed = FALSE;
motordata[z].v24 = FALSE;
motordata[z].cal_pos = V810_MS_CAL;
motordata[z].pot.limit_change = V810_MS_CHANGE;
motordata[z].pot.limit_span = V810_MS_SPAN;
motordata[z].pot.limit_offset_l = V810_MS_OFFSET_L;
motordata[z].pot.limit_offset_h = V810_MS_OFFSET_H;
}
motordata[0].active = TRUE; // x motor for SLIT position
}
if (part == V810_M) {
term_time();
mode.display = e220_m_display;
putrs2USART("\x1b[7m Init VISTA MANIPULATOR.\x1b[0m\r\n");
for (z = 0; z < MAX_MOTOR; z++) {
motordata[z].cal_pos = V810_M_CAL;
motordata[z].pot.limit_change = V810_M_CHANGE;
motordata[z].pot.limit_span = V810_M_SPAN;
motordata[z].pot.limit_offset_l = V810_M_OFFSET_L;
motordata[z].pot.limit_offset_h = V810_M_OFFSET_H;
}
// motordata[2].active = FALSE;
motordata[0].pot.limit_span = V810_M_SPAN_X;
motordata[1].pot.limit_span = V810_M_SPAN_Y;
motordata[2].pot.limit_span = V810_M_SPAN_Z;
}
if (part == VIISION_M) {
term_time();
mode.display = viision_m_display;
putrs2USART("\x1b[7m Init VIISION MANIPULATOR.\x1b[0m\r\n");
for (z = 0; z < MAX_MOTOR; z++) {
motordata[z].pot.limit_change = VIISION_M_CHANGE;
motordata[z].pot.limit_span = VIISION_M_SPAN;
motordata[z].pot.limit_offset_l = VIISION_M_OFFSET_L;
motordata[z].pot.limit_offset_h = VIISION_M_OFFSET_H;
}
motordata[1].active = FALSE; // no Y motor
motordata[0].v24 = TRUE;
motordata[2].v24 = TRUE;
}
if (part == E220E500_M) {
term_time();
mode.display = e220_m_display;
putrs2USART("\x1b[7m Init E220E500 MANIPULATOR. \x1b[0m\r\n");
for (z = 0; z < MAX_MOTOR; z++) {
}
motordata[1].pot.limit_span = E220E500_M_SPAN_Y; // Y motor special span setting
}
if (part == GSD_M) {
term_time();
mode.display = gsd_m_display;
putrs2USART("\x1b[7m Init GSD MANIPULATOR. \x1b[0m\r\n");
for (z = 0; z < MAX_MOTOR; z++) {
motordata[z].slow = TRUE;
motordata[z].slow_only = TRUE;
motordata[z].v24 = TRUE;
motordata[z].active = FALSE;
}
motordata[0].active = TRUE; // X motor for ALL positions
}
if (part == HELP_M) {
term_time();
mode.display = help_m_display;
putrs2USART("\x1b[7m Help Menu. \x1b[0m\r\n");
for (z = 0; z < MAX_MOTOR; z++) {
motordata[z].active = TRUE;
}
mode.info_only = TRUE;
}
if (part == VIISION_MS) {
term_time();
mode.display = viision_ms_display;
putrs2USART("\x1b[7m Init VIISION MASS SLIT. \x1b[0m\r\n");
for (z = 0; z < MAX_MOTOR; z++) {
motordata[z].slow = FALSE;
motordata[z].slow_only = FALSE;
motordata[z].active = FALSE;
motordata[z].reversed = TRUE;
motordata[z].v24 = TRUE;
motordata[z].cal_pos = VIISION_MS_CAL;
motordata[z].pot.limit_change = VIISION_MS_CHANGE;
motordata[z].pot.limit_span = VIISION_MS_SPAN;
motordata[z].pot.limit_offset_h = VIISION_MS_OFFSET_H;
motordata[z].pot.limit_offset_l = VIISION_MS_OFFSET_L;
}
motordata[1].active = TRUE; // Y motor for SLIT position
}
if (part == VARIAN_V) { // Open SW on Z pot, Closed SW on X POT, Y POT is not used
term_time();
mode.display = varian_v_display;
putrs2USART("\x1b[7m Init VARIAN TYPE VALVE. \x1b[0m\r\n");
for (z = 0; z < MAX_MOTOR; z++) {
motordata[z].active = FALSE;
motordata[z].v24 = TRUE;
motordata[z].slow = FALSE;
}
motordata[1].active = TRUE; // Y motor for VALVE position
mode.on_off_only = TRUE;
}
e_crit(); // after last command
}
