/* ROVotron transmitter configuration program (C) 2010 David Forbes Revision history 2010-03-21 DF Split config.c from rtxa.c, writing code 2010-03-22 DF Finished writing first try at the configuration code Fixed typos, compiles, debugged, runs good 2010-03-23 DF Making config a struct, writing telemetry config & display Added configuration saving to EEPROM Added code to change big numbers fast based on pressure 2010-03-24 DF Added checksum to eedata 2010-03-28 DF Writing servo rate code, changing config for it 2010-04-16 DF Fixed typo in min values to get negative gains 2010-04-21 DF Added telem units, reduced decimals in telem display Known bugs: Things to do: Add limit switches to reply message and to new servo limit mode Terminology: Buttons are the 14 user pushbuttons on the PS2 controller (not Start or Select) Analogs are the ten analog axes of control on the PS2 (button pairs or sticks) Switches are the ten ROV switch channels Motors are the ten ROV motor channels Servos are the four servo outputs Primary is the main axis that moves the motor, e.g. fore-aft for thruster pairs Secondary is another axis that moves the motor, e.g. sideways for thruster pairs volts are the four raw 15-bit ADC outputs from the ROV telems are the four 16-bit telemetry values, mapped into user units Motor parameters: Gain is the multiplication factor for an analog to a motor, -99 to +99 mode is the speed mode: one of two below 0 OFF is a deactivated motor or servo 1 SPEED is no conversion. Passes analog to motor or servo directly 2 RATE causes analog to be integrated to make servo position or motor speed in Rate mode, Gain is the speed at which a full stick increases the servo position Switch parameters: mode is the switch actuation method: one of the two below 0 OFF is a deactivated switch 1 MOM is momentary: switch stays on only while pushing button 2 ONOFF is paired: one button turns switch on, another button turns it off The configuration menu has four screens: 0 Motor 1 Servo 2 Switch 3 Telem. Each of these has different fields to be configured. The fields are as follows: Motor: 0 screen 0=motor 1 device 0..9 motors 2 mode 0=off, 1=speed, 2=rate 3 label index to label string 4 mot_pctl[device] 0..NANALOG 5 mot_pgain[device] -99 to +99 6 mot_sctl[device] 0..NANALOG 7 mot_sgain[device] -99 to +99 Servo Pos.: 0 screen 1=servo 1 device 0..3 servos 2 mode 0=off, 1=pos, 3=limit 3 label index to label string 4 pwm_ctl[device] 0..NANALOG 5 pwm_gain[device] -99 to +99 6 pwm_sctl[device] 0..NANALOG 7 pwm_range[device] 0 to +99 Servo Rate: 0 screen 1=servo 1 device 0..3 servos 2 mode 2=rate 3 label index to label string 4 pwm_ctl[device] 0..NANALOG 5 pwm_gain[device] -99 to +99 6 pwm_zbtn[device] 0..NBUTTONS 7 pwm_range[device] 0 to +99 This is optional if we need to add it. No where on screen to show it! 8 pwm_zero[device] -99 to +99 Switch: 0 screen 2=switch 1 device 0..9 switches 2 mode 0=off, 1=mom, 2=onoff 3 label index to label string 4 on_btn[device] 0..NBUTTONS 5 off_btn[device] 0..NBUTTONS Telem: 0 screen 3=telem. 1 device 0..3 analog inputs 2 mode 0..4 enable, decimal point position+1 3 label index to label string 4 tel_shift[device] 0..14 # bits to shift analog value right 5 tel_zero[device] ** int for offset of analog value 6 tel_gain[device] ** int for gain of analog value ** special cases; can't use standard modify_field code for these The menu navigation is done by moving among the fields with the four left diamond buttons and changing the value of the selected field with the right diamond up/down buttons. The START button exits configuration mode with a saving prompt. The first field, screen, determines which screen is displayed, and therefore which other fields exist. A selected field is surrounded by square brackets to identify it. This highlighting method is used because the standard LCD display doesn't have any other way to indicate selected text. Got any better ideas? Each screen has a navigation array and a display routine. Each field type has a modification routine to change the stored value and keep it within range. The modification moves faster for larger values, so there are three modification rates. The three field types are: 0 index modify by 1 each time button pushed. 1 char modify by amount in char_pressure array based on button pressure/0x10. 2 int modify by amount in int_pressure array based on button pressure/0x10. */ // Variables for configuration menus char prev_buttons[NBUTTONS]; // storage for button push detector char dummy; // no-op for fchar ield pointers int dummyi; // no-op for int field pointers char field = 0; // which field on screen as described above char screen = 0; // which screen we're working on char device = 0; // the hardware being affected by the configuration char mode = 0; // o=off, 1 and up are screen-dependent char label = 0; // label index for naming ROV devices // Button pressure to modification rate maps // These give exponentially faster change the harder you press. rom const char char_pressure[] = {0, 0, 1, 1, 1, 2, 2, 3, 4, 5, 6, 6, 8, 10, 10, 10}; rom const int int_pressure[] = {0, 0, 1, 1, 1, 2, 5, 10, 10, 50, 50, 100, 250, 250, 1000, 1000}; /* Field navigation control arrays There are four direction buttons on the left diamond; each can select a different field. So the control code needs to know the geometry of the menu screen. Screen field layouts: Motor, Servo Switch Telemetry 0 1 2 0 1 2 0 1 2 3 3 3 3 3 3 3 3 3 4 5 4 4 5 6 6 7 5 */ // Next field when pushing the UP button // usage: new_field = up_field[screen][field]; // current field: // 0 1 2 3 4 5 6 7 rom const char up_field[4][8] = { {0, 1, 2, 1, 3, 3, 4, 5}, // motor screen {0, 1, 2, 1, 3, 3, 4, 5}, // servo screen {0, 1, 2, 1, 3, 4, 0, 0}, // switch screen {0, 1, 2, 1, 3, 3, 3, 0}}; // telemetry screen // Next field when pushing the DOWN button // current field 0 1 2 3 4 5 6 7 rom const char dn_field[4][8] = { {3, 3, 3, 4, 6, 7, 6, 7}, // motor screen {3, 3, 3, 4, 6, 7, 6, 7}, // servo screen {3, 3, 3, 4, 5, 5, 0, 0}, // switch screen {3, 3, 3, 4, 4, 5, 6, 0}}; // telemetry screen // Next field when pushing the LEFT button // current field 0 1 2 3 4 5 6 7 rom const char lf_field[4][8] = { {0, 0, 1, 3, 4, 4, 6, 6}, // motor screen {0, 0, 1, 3, 4, 4, 6, 6}, // servo screen {0, 0, 1, 3, 4, 5, 0, 0}, // switch screen {0, 0, 1, 3, 4, 4, 5, 0}}; // telemetry screen // Next field when pushing the RIGHT button // current field 0 1 2 3 4 5 6 7 rom const char rt_field[4][8] = { {1, 2, 2, 3, 5, 5, 7, 7}, // motor screen {1, 2, 2, 3, 5, 5, 7, 7}, // servo screen {1, 2, 2, 3, 4, 5, 0, 0}, // switch screen {1, 2, 2, 3, 5, 6, 6, 0}}; // telemetry screen #pragma idata idata1 // split up storage to fit each on a page // These are for field 0, the screen field /* These names identify the four menus plus running mode. */ rom const char *screen_names[4] = { "Motor ", "Servo ", "Switch", "Telem."}; // These are for field 1, the device field /* These names identify the ten motors. */ rom const char *motor_dev_names[] = { "1A ", // first motor on first board "1B ", // second motor on first board "2A ", // first motor on second board "2B ", "3A ", "3B ", "4A ", "4B ", "5A ", "5B "}; /* These names identify the ten switches. */ rom const char *switch_dev_names[] = { "1NO", // first switch on first board "1NC", // second switch on first board "2NO", // first switch on second board "2NC", "3NO", "3NC", "4NO", "4NC", "5NO", "5NC"}; /* These names identify the four PWM servos. */ rom const char *servo_dev_names[] = { " 1 ", // first servo " 2 ", " 3 ", " 4 "}; // These are for field 2, the mode field /* These names identify the three motor modes. */ rom const char *motor_mode_names[] = { "-off-", // zero is an unused motor "Speed", // speed control "Rate "}; // rate of speed change /* These names identify the three servo modes. */ rom const char *servo_mode_names[] = { "-off-", // zero is an unused servo "Pos. ", // position control: analog -> servo PWM "Rate ", // rate of change: analog -> integrator -> servo PWM "Limit"}; // Continuous rotation speed with limit switches: analog -> servo PWM /* These names identify the three switch modes. */ rom const char *switch_mode_names[] = { "-off-", // zero is an unused switch "Mom. ", // momentary with one button "OnOff"}; // on-off with two buttons /* These names identify the four decimal point locations. */ rom const char *telem_mode_names[] = { "-off-", // zero is an unused input "0000.", "000.0", "00.00"}; // These are for field 3, the label field // These are ROV-dependent for now. // Ideally, this list would be universal. // These names identify the motors to be driven. rom const char *motor_label_names[] = { " -- unassigned -- ", "L fore-aft drive ", "R fore-aft drive ", // 2 "rear up-down drive", "side-side drive ", // 4 "L up-down drive ", "R up-down drive " // 6 "claw close ", "claw rotate ",}; // 8 #define NMOTOR_LABELS 8 // These names identify the servos to be driven. rom const char *servo_label_names[] = { " -- unassigned -- ", "camera up-down ", "widgety thing "}; #define NSERVO_LABELS 2 // These names identify the switches to be flipped. rom const char *switch_label_names[] = { " -- unassigned -- ", "Torpedo arming ", "Torpedo 1 launch ", "Torpedo 2 launch ", "Torpedo 3 launch "}; #define NSWITCH_LABELS 4 // These names identify the telemetry units. rom const char *telem_label_names[] = { " --- ", " m Depth ", " mm Depth", " deg C ", " deg F ", " atm Pres", " Volts ", " Amperes "}; #define NTELEM_LABELS 7 // These are for fields 4 and up, the control axis /* These names identify the ten sources of analog values. */ rom const char *analog_names[] = { "--off--", // zero is an undriven output "L Joy X", "L Joy Y", "R Joy X", "R Joy Y", "L Pad Y", "L Pad X", "R Pad Y", "R Pad X", "L Trig ", "R Trig "}; /* These names identify the sixteen buttons. */ rom const char *button_names[] = { "--off--", // zero is an undriven output "L Up ", "L Down ", "L Left ", "L Right", "Triangl", "Circle ", "Cross ", "Square ", "LTrig 1", "LTrig 2", "RTrig 1", "RTrig 2", "L Joy ", "R Joy ", "Box 1 ", "Box 2 ", "Box 3 ", "Box 4 ", "Box 5 ", "Box 6 "}; #pragma udata udata3 // split up storage to fit each on a page // --------------------- config section ------------------- // // The configuration arrays are below. // The axis and gain are set to 0 for each unused channel. // this struct is saved to EEData when exiting config, and is loaded // from EEData on startup. It is full of test data for now. struct config { char magic; // so we know it's been initialized // mode of each device char motor_mode[NMOTORS]; char servo_mode[NSERVOS]; char switch_mode[NSWITCHES]; char telem_mode[NVOLTS]; // 28 chars // label indices for each device char motor_label[NMOTORS]; char servo_label[NSERVOS]; char switch_label[NSWITCHES]; char telem_label[NVOLTS]; // 28 chars // analog axis and gain to control each motor char mot_pctl[NMOTORS]; char mot_pgain[NMOTORS]; char mot_sctl[NMOTORS]; char mot_sgain[NMOTORS]; // 40 chars // analog axis and gain to control each servo char pwm_ctl[NSERVOS]; char pwm_gain[NSERVOS]; char pwm_zbtn[NSERVOS]; char pwm_range[NSERVOS]; // 16 chars // button to control each switch char on_btn[NSWITCHES]; char off_btn[NSWITCHES]; // 20 chars // shift code, zero and gain of each telemetry item int tel_zero[NVOLTS]; int tel_gain[NVOLTS]; char tel_shift[NVOLTS]; // 5 chars total=138 chars }; // this lets us write the above struct to/from EEdata. // checksum byte summed over conf is stored in eedats[255]. union char_config{ struct config conf; // the configuration data structure unsigned char conf_chars[200]; // is identical to the char array for saving } cg; // ----------------- field parameter arrays ---------------- // #pragma idata idata3 // split up storage to fit each on a page // type of field: 0=index, 1=char, 2=int. char field_type[4][8] = { {0, 0, 0, 0, 0, 1, 0, 1}, // motors {0, 0, 0, 0, 0, 1, 0, 1}, // servos {0, 0, 0, 0, 0, 0, 0, 0}, // buttons {0, 0, 0, 0, 0, 2, 2, 0}}; // telemetry // field variable pointers indexed by screen and field char *field_ptrs[4][8] = { {&screen, &device, cg.conf.motor_mode, cg.conf.motor_label, cg.conf.mot_pctl, cg.conf.mot_pgain, cg.conf.mot_sctl, cg.conf.mot_sgain}, {&screen, &device, cg.conf.servo_mode, cg.conf.servo_label, cg.conf.pwm_ctl, cg.conf.pwm_gain, cg.conf.pwm_zbtn, cg.conf.pwm_range}, {&screen, &device, cg.conf.switch_mode, cg.conf.switch_label, cg.conf.on_btn, cg.conf.off_btn, &dummy, &dummy}, {&screen, &device, cg.conf.telem_mode, cg.conf.telem_label, cg.conf.tel_shift, &dummy, &dummy, &dummy}}; // gain, zero are ints, so special int* int_field_ptrs[8] = {&dummyi, &dummyi, &dummyi, &dummyi, &dummyi, cg.conf.tel_zero, cg.conf.tel_gain, &dummyi}; // gain, zero are ints, so special // max index values that each field may be set to char max_index_vals[4][8] = { {3, 9, 2, NMOTOR_LABELS, NANALOG-1, 0, NANALOG-1, 0}, {3, 3, 2, NSERVO_LABELS, NANALOG-1, 0, NBUTTONS-1, 0}, // servos {3, 9, 2, NSWITCH_LABELS, NBUTTONS-1, NBUTTONS-1, 0, 0}, // buttons {3, 3, 3, NTELEM_LABELS, 14, 0, 0, 0}}; // telemetry // min values for char fields char min_char_vals[4][8] = { {0, 0, 0, 0, 0, -99, 0, -99}, // motors {0, 0, 0, 0, 0, -99, 0, 0}, // servos {0, 0, 0, 0, 0, 0, 0, 0}, // buttons {0, 0, 0, 0, 0, 0, 0, 0}}; // telemetry // -------------------- EEPROM data storage ------------------------- // // write a byte to EEPROM if it's changed void write_eeprom (unsigned char addr, unsigned char data_byte) { // wait until previous write-access is completed while(EECON1bits.WR == 1); EEADR = addr; EECON1bits.EEPGD = 0; // select "EEPROM" EECON1bits.CFGS = 0; // not the config-registers EECON1bits.FREE = 0; // do not delete before // does new data = already saved data ? EECON1bits.RD = 1; if (EEDATA == data_byte) { // if new = old, do not overwrite. return; } EEDATA = data_byte; EECON1bits.WREN = 1; // set the write enable bit PIR2bits.EEIF = 0; // clears IF EECON2 = 0x55; EECON2 = 0xAA; EECON1bits.WR = 1; // ...and write! } // read a byte from EEPROM unsigned char read_eeprom (unsigned char addr) { // wait until (possible) previous write-access is finished while(EECON1bits.WR == 1); EECON1bits.WREN = 0; // write enable bit clear EEADR = addr; EECON1bits.EEPGD = 0; // select "EEPROM" EECON1bits.CFGS = 0; // not the config-registers EECON1bits.RD = 1; // read out return EEDATA; } // write the configuration struture to the data EEPROM void write_conf_to_eeprom(void) { unsigned char i, val, checksum = 0; unsigned char *ptr; ptr = &cg.conf_chars[0]; for (i=0; i> cg.conf.tel_shift[i]); } } // display the telemetry value on the LCD using a quasi-floating point // This has gotta take less code storage than bringing in the FP library. void display_telem(char chan) { int frac, whole; switch (cg.conf.telem_mode[chan]) { case 0: sprintf(buf, "unused"); case 1: sprintf(buf, "%6d.", telems[chan]); break; case 2: frac = telems[chan] % 10; if (frac < 0) frac = -frac; // no minus signs to right of DP! whole = telems[chan] / 10; sprintf(buf, "%5d.%1d", whole, frac); break; case 3: frac = telems[chan] % 100; if (frac < 0) frac = -frac; whole = telems[chan] / 100; sprintf(buf, "%4d.%02d", whole, frac); break; default: break; } putsXLCD(buf); } // -------------- Configuration screen display ------------- // // display the proper configuration screen void display_screen(void) { while (BusyXLCD()); WriteCmdXLCD(1); // clear screen switch (screen) { case 0: display_motor_screen(); break; case 1: display_servo_screen(); break; case 2: display_switch_screen(); break; case 3: display_telem_screen(); break; } } // Display the screen for configuring a motor void display_motor_screen(void) { // screen, device, mode SetLineXLCD(0); display_field_open(0); putrsXLCD(screen_names[screen]); display_field_close(0); display_field_open(1); putrsXLCD(motor_dev_names[device]); display_field_close(1); display_field_open(2); putrsXLCD(motor_mode_names[cg.conf.motor_mode[device]]); display_field_close(2); // label SetLineXLCD(1); display_field_open(3); putrsXLCD(motor_label_names[cg.conf.motor_label[device]]); display_field_close(3); // primary axis parameters SetLineXLCD(2); putrsXLCD("Pri:"); display_field_open(4); putrsXLCD(analog_names[cg.conf.mot_pctl[device]]); display_field_close(4); display_field_open(5); sprintf(buf, "%+3d\%", cg.conf.mot_pgain[device]); putsXLCD(buf); display_field_close(5); // secondary axis parameters SetLineXLCD(3); putrsXLCD("Sec:"); display_field_open(6); putrsXLCD(analog_names[cg.conf.mot_sctl[device]]); display_field_close(6); display_field_open(7); sprintf(buf, "%+3d\%", cg.conf.mot_sgain[device]); putsXLCD(buf); display_field_close(7); } // Display the screen for configuring a servo void display_servo_screen(void) { // screen, device, mode SetLineXLCD(0); display_field_open(0); putrsXLCD(screen_names[screen]); display_field_close(0); display_field_open(1); putrsXLCD(servo_dev_names[device]); display_field_close(1); display_field_open(2); putrsXLCD(servo_mode_names[cg.conf.servo_mode[device]]); display_field_close(2); // label SetLineXLCD(1); display_field_open(3); putrsXLCD(servo_label_names[cg.conf.servo_label[device]]); display_field_close(3); // primary axis parameters SetLineXLCD(2); putrsXLCD("Gain:"); display_field_open(4); putrsXLCD(analog_names[cg.conf.pwm_ctl[device]]); display_field_close(4); display_field_open(5); sprintf(buf, "%+3d\%", cg.conf.pwm_gain[device]); putsXLCD(buf); display_field_close(5); // secondary axis parameters SetLineXLCD(3); putrsXLCD("Rng: "); // if (cg.conf.servo_mode[device] == 2) { // display zbtn only if rate mode display_field_open(6); putrsXLCD(button_names[cg.conf.pwm_zbtn[device]]); display_field_close(6); // } else // putrsXLCD(" "); display_field_open(7); sprintf(buf, "%+3d\%", cg.conf.pwm_range[device]); putsXLCD(buf); display_field_close(7); } // Display the screen for configuring a switch void display_switch_screen(void) { // screen, device, mode SetLineXLCD(0); display_field_open(0); putrsXLCD(screen_names[screen]); display_field_close(0); display_field_open(1); putrsXLCD(switch_dev_names[device]); display_field_close(1); display_field_open(2); putrsXLCD(switch_mode_names[cg.conf.switch_mode[device]]); display_field_close(2); // label SetLineXLCD(1); display_field_open(3); putrsXLCD(switch_label_names[cg.conf.switch_label[device]]); display_field_close(3); // on button parameters SetLineXLCD(2); putrsXLCD("On: "); display_field_open(4); putrsXLCD(button_names[cg.conf.on_btn[device]]); display_field_close(4); // off button parameters - display only if needed // if (cg.conf.switch_mode[device] == ONOFF) { SetLineXLCD(3); putrsXLCD("Off:"); display_field_open(5); putrsXLCD(button_names[cg.conf.off_btn[device]]); display_field_close(5); // } } // Display the screen for configuring a telemetry item void display_telem_screen(void) { // screen, device, mode SetLineXLCD(0); display_field_open(0); putrsXLCD(screen_names[screen]); display_field_close(0); display_field_open(1); putrsXLCD(servo_dev_names[device]); display_field_close(1); display_field_open(2); putrsXLCD(telem_mode_names[cg.conf.telem_mode[device]]); display_field_close(2); // label SetLineXLCD(1); display_telem(device); // show current telemetry reading display_field_open(3); putrsXLCD(telem_label_names[cg.conf.telem_label[device]]); display_field_close(3); // volts to telems conversion parameters SetLineXLCD(2); display_field_open(4); sprintf(buf, "%2d", cg.conf.tel_shift[device]); putsXLCD(buf); display_field_close(4); display_field_open(5); sprintf(buf, "%+6d", cg.conf.tel_zero[device]); putsXLCD(buf); display_field_close(5); display_field_open(6); sprintf(buf, "%+6d", cg.conf.tel_gain[device]); putsXLCD(buf); display_field_close(6); // labels for above SetLineXLCD(3); putrsXLCD("Shift Zero Gain "); } // Put brackets around the field that's being updated void display_field_open(char disp_field) { while (BusyXLCD()); if (field == disp_field) putcXLCD('['); else putcXLCD(' '); } void display_field_close(char disp_field) { while (BusyXLCD()); if (field == disp_field) putcXLCD(']'); else putcXLCD(' '); } // ------------------- Field modification code ------------------- // // move to new field based on movement from left diamond buttons void move_field(char button) { switch (button) { case L_UP_B: field = up_field[screen][field]; break; case L_DOWN_B: field = dn_field[screen][field]; break; case L_LEFT_B: field = lf_field[screen][field]; break; case L_RIGHT_B: field = rt_field[screen][field]; break; default: // other buttons do nothing here break; } } // modify an index-type field value based on right diamond buttons void modify_index_field(char button) { char *field_p; field_p = field_ptrs[screen][field]; if (field >= 2) // if it's a device-specific field, field_p += device; // modify this device's parameter switch (button) { case TRIANGLE_B: // increment if (*field_p < max_index_vals[screen][field]) *field_p += 1; break; case CROSS_B: // decrement if (*field_p > 0) *field_p -= 1; break; default: // other buttons don't do anything at all here break; } } // modify a char-type field value based on right diamond buttons // uses button pressure in a non-linear way for speedy coarse adjustment void modify_char_field(void) { char *field_p; int val; // big variable for modification in case overflow field_p = field_ptrs[screen][field] + device; val = *field_p; // copy the value to modify val += char_pressure[PS2_TrianglePress>>4]; // increase value as needed if (val > 99) val = 99; val -= char_pressure[PS2_CrossPress>>4]; // decrease value as needed if (val < min_char_vals[screen][field]) val = min_char_vals[screen][field]; *field_p = (char)val; // copy the new value to device parameter } // modify an int-type field value based on right diamond buttons // uses button pressure in a non-linear way for speedy coarse adjustment void modify_int_field(void) { int *field_p; short long val; // big variable for modification in case overflow field_p = int_field_ptrs[field] + device; val = *field_p; // copy the value to modify val += int_pressure[PS2_TrianglePress>>4]; // increase value as needed if (val > 32767) val = 32767; val -= int_pressure[PS2_CrossPress>>4]; // decrease value as needed if (val < -32767) val = -32767; *field_p = (int)val; // copy the new value to device parameter } // return the button number of the highest just-pushed button found char get_button_push(void) { char i, pushed = 0; for (i=1;i max for screen device = 0; last_screen = screen; display_screen(); Delay1KTCYx(255); //Delay of 15ms Delay1KTCYx(255); //Delay of 15ms translate_to_telems(volts); // to make analog display valid // get buttons from handset rw_ps2_packet(0, (rom char *)command_poll_18); translate_response_to_controls(); pushed = get_button_push(); // see if a button is pressed switch (field_type[screen][field]) { case 0: // index type modify_index_field(pushed); break; case 1: modify_char_field(); // char field type break; case 2: modify_int_field(); // int field type break; } move_field(pushed); // move highlight if motion } while (!PS2_Start); // save configuration data before resuming ROV operation while (BusyXLCD()); WriteCmdXLCD(1); // clear screen SetLineXLCD(1); putrsXLCD(" Saving"); SetLineXLCD(2); putrsXLCD("configuration data"); write_conf_to_eeprom(); // save configuration in EEPROM delay_2sec(); return; }