bool jointData::read(yarp::os::idl::WireReader& reader) {
if (!read_jointPosition(reader)) return false;
if (!read_jointPosition_isValid(reader)) return false;
if (!read_jointVelocity(reader)) return false;
if (!read_jointVelocity_isValid(reader)) return false;
if (!read_jointAcceleration(reader)) return false;
if (!read_jointAcceleration_isValid(reader)) return false;
if (!read_motorPosition(reader)) return false;
if (!read_motorPosition_isValid(reader)) return false;
if (!read_motorVelocity(reader)) return false;
if (!read_motorVelocity_isValid(reader)) return false;
if (!read_motorAcceleration(reader)) return false;
if (!read_motorAcceleration_isValid(reader)) return false;
if (!read_torque(reader)) return false;
if (!read_torque_isValid(reader)) return false;
if (!read_pwmDutycycle(reader)) return false;
if (!read_pwmDutycycle_isValid(reader)) return false;
if (!read_current(reader)) return false;
if (!read_current_isValid(reader)) return false;
if (!read_controlMode(reader)) return false;
if (!read_controlMode_isValid(reader)) return false;
if (!read_interactionMode(reader)) return false;
if (!read_interactionMode_isValid(reader)) return false;
return !reader.isError();
}
void user_loop(void)
{
unsigned char channel;
/* loop over all HV channels */
for (channel=0 ; channel<N_HV_CHN ; channel++) {
watchdog_refresh(0);
if ((user_data[0].control & CONTROL_IDLE) == 0) {
/* read back HV and current */
read_hv(channel);
read_current(channel);
/* check for current trip */
check_current(channel);
/* do ramping and regulation */
ramp_hv(channel);
regulation(channel);
/* set voltage regularly, in case DAC hot HV spike */
set_hv(channel, user_data[channel].u_dac);
}
}
// read_temperature();
}
uchar read_random(uint random_addr)
// 在指定地址读取
{
start();
I2C_tx(OP_WRITE);
I2C_tx(random_addr>>8);
I2C_tx(random_addr);
return(read_current());
}
void scroll_test(){
loop_count = 0;
loop_average = 0;
while (loop_count < 10)
{
read_current();
read_temp();
printf("mV===>(%4.3f)",current_float);
loop_average = loop_average + current_float;
printf(" deg C===>(%3.2f)",temp_float);
printf(" deg F===>(%3.2f)\n\r",temp_float_faren);
loop_count ++;
delay_ms(1000);
}
loop_average = loop_average / 10;
printf("mV average===>(%4.3f)\n\r",loop_average);
}
void ZX_Handler(uint32_t id, uint32_t mask) {
ioport_toggle_pin_level(LED1_GPIO);
ioport_toggle_pin_level(FP_LED2_GPIO);
if (zx_count == 60) {
zx_count = 0;
epoch++;
}
zx_count++;
read_voltage();
read_current();
read_period();
char* measurement = create_measurement_string();
printf("%s\r\n", measurement);
free(measurement);
}
void check_current(unsigned char channel)
{
#ifdef HARDWARE_TRIP
if (user_data[channel].i_meas > user_data[channel].i_limit || // "software" check
hardware_current_trip(channel)) { // "hardware" check
if (trip_time[channel] == 0)
trip_time[channel] = time();
/* zero output voltage */
set_hv(channel, 0);
u_actual[channel] = 0;
user_data[channel].u_dac = 0;
/* stop possible ramping */
chn_bits[channel] &= ~DEMAND_CHANGED;
chn_bits[channel] &= ~RAMP_UP;
chn_bits[channel] &= ~RAMP_DOWN;
user_data[channel].status &= ~(STATUS_RAMP_UP | STATUS_RAMP_DOWN);
/* raise trip flag */
if ((user_data[channel].status & STATUS_ILIMIT) == 0) {
user_data[channel].status |= STATUS_ILIMIT;
user_data[channel].trip_cnt++;
}
}
#else
if (user_data[channel].i_meas > user_data[channel].i_limit &&
user_data[channel].u_dac > 40) {
unsigned char i;
/* ADC could be crashed, so reset it and read current again */
for (i=0 ; i<10 ; i++) {
reset_adc();
delay_ms(50);
read_hv(channel);
if (read_current(channel))
break;
}
/* now test again for trip */
if (user_data[channel].i_meas > user_data[channel].i_limit) {
if (trip_time[channel] == 0)
trip_time[channel] = time();
/* ramp down */
chn_bits[channel] &= ~DEMAND_CHANGED;
chn_bits[channel] &= ~RAMP_UP;
user_data[channel].status &= ~STATUS_RAMP_UP;
if (user_data[channel].ramp_down > 0) {
/* ramp down if requested */
chn_bits[channel] |= RAMP_DOWN;
user_data[channel].status |= STATUS_RAMP_DOWN;
} else {
/* otherwise go to zero immediately */
set_hv(channel, 0);
u_actual[channel] = 0;
user_data[channel].u_dac = 0;
/* stop possible ramping */
chn_bits[channel] &= ~RAMP_DOWN;
user_data[channel].status &= ~STATUS_RAMP_DOWN;
}
/* raise trip flag */
if ((user_data[channel].status & STATUS_ILIMIT) == 0) {
user_data[channel].status |= STATUS_ILIMIT;
user_data[channel].trip_cnt++;
}
}
}
#endif
/* check for trip recovery */
if ((user_data[channel].status & STATUS_ILIMIT) &&
user_data[channel].trip_cnt < user_data[channel].trip_max &&
time() >= trip_time[channel] + user_data[channel].trip_time*100) {
/* clear trip */
user_data[channel].status &= ~STATUS_ILIMIT;
trip_time[channel] = 0;
#ifdef HARDWARE_TRIP
reset_hardware_trip();
#endif
/* force ramp up */
chn_bits[channel] |= DEMAND_CHANGED;
}
if (user_data[channel].status & STATUS_ILIMIT)
led_blink(channel, 3, 100);
}
void user_loop(void)
{
unsigned char xdata channel, i;
/* loop over all HV channels */
for (channel=0 ; channel<N_HV_CHN ; channel++) {
watchdog_refresh(0);
if ((user_data[0].control & CONTROL_IDLE) == 0) {
/* set current limit if changed */
if (chn_bits[channel] & CUR_LIMIT_CHANGED) {
set_current_limit(user_data[channel].i_limit);
chn_bits[channel] &= ~CUR_LIMIT_CHANGED;
trip_reset = 1;
}
/* read back HV and current */
read_hv(channel);
read_current(channel);
/* check for current trip */
check_current(channel);
/* do ramping and regulation */
ramp_hv(channel);
regulation(channel);
/* set voltage regularly, in case DAC got HV spike */
set_hv(channel, user_data[channel].u_dac);
}
#ifdef HARDWARE_TRIP
if (trip_reset) {
reset_hardware_trip();
trip_reset = 0;
}
#endif
/* if crate HV switched off, set DAC to zero */
if (SW1) {
for (i = 0 ; i<N_HV_CHN ; i++ ) {
user_data[i].u_dac = 0;
user_data[i].status |= STATUS_DISABLED;
u_actual[i] = 0;
set_hv(i, 0);
}
old_sw1 = 1;
}
/* if crate HV switched on, indicated changed demand value*/
if (!SW1 && old_sw1) {
for (i = 0 ; i<N_HV_CHN ; i++ ) {
chn_bits[i] |= DEMAND_CHANGED;
user_data[i].status &= ~STATUS_DISABLED;
}
old_sw1 = 0;
}
}
/* reset ADC once all channels have been read */
reset_adc();
// read_temperature();
}
static void edit_marquee(void) {
uint8_t idx = 0;
char current;
marquee_msgptr = stored_config.data;
display[0] = 0;
display[1] = 0;
display[7] = 0;
// Load and show the first character
current = read_current(idx);
draw_character(current);
while(state == STATE_NORMAL) {
if(keypresses & KEY_LEFT) {
if(idx > 0) {
write_dirty(idx, current);
idx--;
current = read_current(idx);
for(uint8_t i = 0; i < 8; i++) {
shift_right();
if(i > 0 && i < 6) {
display[0] = read_font_column(current, 5 - i);
} else {
display[0] = 0;
}
_delay_ms(50);
}
}
keypresses &= ~KEY_LEFT;
} else if(keypresses & KEY_RIGHT) {
if(idx < MAX_DATA_LENGTH && current != '\0') {
write_dirty(idx, current);
idx++;
current = read_current(idx);
for(uint8_t i = 0; i < 8; i++) {
shift_left();
if(i > 1 && i < 7) {
display[7] = read_font_column(current, i - 2);
} else {
display[7] = 0;
}
_delay_ms(50);
}
}
keypresses &= ~KEY_RIGHT;
} else if(keypresses & KEY_UP) {
if(current < 255)
current++;
draw_character(current);
keypresses &= ~KEY_UP;
} else if(keypresses & KEY_DOWN) {
if(current > 0)
current--;
draw_character(current);
keypresses &= ~KEY_DOWN;
} else {
continue;
}
}
write_dirty(idx, current);
}
void main(){
setup_oscillator( OSC_8MHZ );
setup_adc_ports(sAN6|VSS_VDD);
setup_adc(ADC_CLOCK_INTERNAL);
setup_counters(RTCC_INTERNAL,RTCC_DIV_1);
setup_timer_1(T1_DISABLED);
setup_comparator(NC_NC_NC_NC);
setup_vref(FALSE);
//Software workaround for the power switch floating
onewire_init();
onewire_sendbyte(0xCC);
onewire_sendbyte(0x6C); //Write Data Command
onewire_sendbyte(0x31); //Eeprom address but actually gets written to Shadow Ram
onewire_sendbyte(0xE7); //Value to make PMOD1 SWEN=0 RNAOP=0
//Copy the shadow Ram written above over to actual EEPROM
onewire_init();
onewire_sendbyte(0xCC);
onewire_sendbyte(0x48); //send the copy command
onewire_sendbyte(0x30); //copy shadow ram to the block containing 31
while(true){
/*-------------------------------------------------------------------
Pull Reading From Temp Probe
-------------------------------------------------------------------*/
//Use the following to determine the state of the one wire net
//Will report if device present, not, or shorted
//Comment out rest of code
//onewire_init_with_error_check();
//read_status();
//printf("status byte is ====>(%x)\n\r",status);
printf("Please enter a command (h for help):\n\r");
command = getc(); //Gets a key from the keyboard
switch (command){
case 'h' :
printf("Type any of the following commands:\n\r");
printf("h This Help Message\n\r");
printf("C Ambiant Temp in deg. C\n\r");
printf("c Ambiant Temp in deg. C(No Formatting)\n\r");
printf("F Ambiant Temp in deg. F\n\r");
printf("f Ambiant Temp in deg. F(No Formatting)\n\r");
printf("N 64 bit node address in Hex\n\r");
printf("K Thermo millivolts\n\r");
printf("k Thermo millivolts(No Formatting)\n\r");
printf("s One line scroll test\n\r");
break;
case 'C' :
read_temp();
printf(" deg C===>(%3.2f)\n\r",temp_float);
break;
case 'c' :
read_temp();
printf("%3.2f\n\r",temp_float);
break;
case 'F' :
read_temp();
printf(" deg F===>(%3.2f)\n\r",temp_float_faren);
break;
case 'f' :
read_temp();
printf("%4.2f",temp_float_faren);
break;
case 'K' :
read_current();
printf("mV===>(%4.3f)\n\r",current_float);
break;
case 'k' :
read_current();
printf("%4.3f\n\r",current_float);
break;
case 's' :
scroll_test();
break;
default :
printf("Not a valid command:\n\r");
}
delay_ms(1000);
}
}
