From 2596a594f6ed084ae90b197c61d9dae730e2d490 Mon Sep 17 00:00:00 2001 From: JamesNewton Date: Wed, 22 Feb 2023 12:31:55 -0800 Subject: [PATCH 1/5] Start bytecode language Including opcodes, registers, saving to EEPROM, convert address to value, etc... --- Arduino_Dynamixel_Controller.ino | 80 +++++++++++++++++++++++++++----- 1 file changed, 68 insertions(+), 12 deletions(-) diff --git a/Arduino_Dynamixel_Controller.ino b/Arduino_Dynamixel_Controller.ino index 5f70d31..af26bb7 100644 --- a/Arduino_Dynamixel_Controller.ino +++ b/Arduino_Dynamixel_Controller.ino @@ -95,11 +95,13 @@ Examples: */ #define BAUD 115200 -#define DYNAMIXEL_SUPPORT +//#define DYNAMIXEL_SUPPORT //Note: If enabled, the Dynamixel Arduino shield is required and the standard Arduino UNO serial port is NOT functional! //You must install the USB to Serial interface on the Dynamixel Shield and use that for communication. RTFM #define STEPPER_SUPPORT +#include + #ifdef DYNAMIXEL_SUPPORT #include @@ -197,6 +199,11 @@ AccelStepper stepper(step_forward, step_back); //avoids setting up the pins now. long n,p,d, radix; int sign; char cmd; +char op; +bool quoting; +#define REG_SIZE 26 +long reg[REG_SIZE]; +byte *mem; //will point into reg /* https://playground.arduino.cc/Code/PwmFrequency @@ -285,6 +292,10 @@ void setup() { d=2; //delay. Default is 2uS or 250KHz radix=10; sign=1; + mem = reg[0]; + EEPROM.get(0, reg['m'-'a']); //load up the count of memory used + op = 0; + quoting = false; #ifdef STEPPER_SUPPORT stepper.setAcceleration(DEFAULT_ACCEL);stepper.setMaxSpeed(DEFAULT_VELOCITY); dir_pin = DEFAULT_DIR_PIN; @@ -295,12 +306,53 @@ void setup() { void loop(){ while (DEBUG_SERIAL.available() > 0) { //if data has arrived int c = DEBUG_SERIAL.read(); //get the data + cmd = char(c); + if ('\"'==cmd) { //quote + if (!quoting) { //this is a new quote + if (NUM_DIGITAL_PINS >= n || REG_SIZE+NUM_DIGITAL_PINS <= n) {DEBUG_SERIAL.print("reg?");continue;} + reg[n-NUM_DIGITAL_PINS]=reg['m'-'a']; //reg being defined points to mem at reg m + quoting = true; + } else { //end of a quote + EEPROM.put(0, reg['m'-'a']); //save the number of bytes we've used + quoting = false; + } + continue; //in either case, we are done here + } + if ('\n'==cmd || '\r'==cmd) { //EOL + if (NUM_DIGITAL_PINS < p && REG_SIZE+NUM_DIGITAL_PINS > p) { //p is a register + reg[p-NUM_DIGITAL_PINS]=n; //TODO check for an ':' op as well. actually do a switch here. + p=0; //for now + } + n=0; cmd=0;//clear command and value (but not p) at end of line. + d=0; //delay doesn't last past one line + quoting = false; //no multi-line quoting, for now... maybe later + continue; //loop now, no delay + } + if (quoting) { //if we are quoting, just put it away + DEBUG_SERIAL.print(reg['m'-'a']); + EEPROM.update(reg['m'-'a']++,cmd); //add the current address, then increment it + DEBUG_SERIAL.print(':'); + DEBUG_SERIAL.println(cmd); + continue; //and do nothing more + } + if ('@'==cmd && n < EEPROM.length()) { //value AT address + n=EEPROM.read(n); //Get the value out of memory at that address + continue; + } + if ('!'==cmd && p < EEPROM.length()) { //Write value to address + EEPROM.update(p,n); //put n into EEPROM at p + continue; + } if ('0' <= c && c <= '9') { //if it's a digit n = (c-'0')*sign + n*radix; //add it to n, shift n up 1 digit sign = 1; //in case it was negative continue; //and loop } - cmd = char(c); //wasn't a number, must be a command + if ('a' <= cmd && cmd <= 'z') { //if it's a variable + n = n + (c - 'a') + NUM_DIGITAL_PINS; //store the address + //note that n is folded in. e.g. 1a is b + continue; //and loop + } if (' '==cmd || '\t'==cmd) { continue;} //whitespace does nothing if (','==cmd) { p=n; n=0; continue;} //save n to p, clear n, loop if (0==n) { @@ -336,15 +388,23 @@ void loop(){ DEBUG_SERIAL.print(dxl.getPresentVelocity(servo_id, UNIT_RPM)); #endif } - else { //specific pin + else { //specific pin or address DEBUG_SERIAL.print("\""); DEBUG_SERIAL.print(n); DEBUG_SERIAL.print("\":["); if (DXL_DIR_PIN != n) { //don't mess with the servo pins - DEBUG_SERIAL.print(digitalRead(n)); //just that one pin + int value = 0; + if (NUM_DIGITAL_PINS > n) { //it's a pin + value = digitalRead(n); + } else { //it's an address + if (REG_SIZE+NUM_DIGITAL_PINS < n) n = REG_SIZE+NUM_DIGITAL_PINS; + value = reg[n-NUM_DIGITAL_PINS]; //to a register + } + DEBUG_SERIAL.print(value); //just that one value if (ANALOG_PINS > n) { //if there is an analog channel + value = analogRead(p); //use that instead. DEBUG_SERIAL.print(","); - DEBUG_SERIAL.print(analogRead(p)); //also return it + DEBUG_SERIAL.print(value); //also return it } } #ifdef DYNAMIXEL_SUPPORT @@ -358,10 +418,10 @@ void loop(){ #endif } DEBUG_SERIAL.println("]}"); - DEBUG_SERIAL.write(04); //EOT + //DEBUG_SERIAL.write(04); //EOT //sending EOT can help OS serial device drivers return data instead of waiting forever for the file to end. //https://stackoverflow.com/questions/50178789/signal-end-of-file-in-serial-communication - //DEBUG_SERIAL.print("\n"); //new line can help after EOT to tigger xmit on OS serial handler + DEBUG_SERIAL.print("\n"); //new line can help after EOT to tigger xmit on OS serial handler break; case '-': case 'H': //set pin n output high @@ -475,11 +535,7 @@ void loop(){ } break; #endif - case '\n': - case '\r': - n=0; cmd=0; //clear command and value at end of line. - continue; //loop now, no delay - break; //shouldn't get here + default: DEBUG_SERIAL.print("\""); DEBUG_SERIAL.print(n); From b60031b53970df50533ea34e635d9c3f4a2a8ca6 Mon Sep 17 00:00:00 2001 From: JamesNewton Date: Sat, 15 Jun 2024 22:03:45 -0700 Subject: [PATCH 2/5] Add define, stack, call, return. Temp remove Servo/Stepper Add define (via quote), stack, call, return. Remove Servo/Stepper to simplify debugging --- Arduino_Dynamixel_Controller.ino | 532 +++++++++---------------------- 1 file changed, 153 insertions(+), 379 deletions(-) diff --git a/Arduino_Dynamixel_Controller.ino b/Arduino_Dynamixel_Controller.ino index af26bb7..9499579 100644 --- a/Arduino_Dynamixel_Controller.ino +++ b/Arduino_Dynamixel_Controller.ino @@ -1,250 +1,63 @@ -/* -Arduino_Dynamixel_Controller.ino -//20170512 initial version -//20170517 uS timing (was mS), vars are longs, i2C start/stop, and clock IN data w/. -//20201002 Returns valid JSON. -//20201005 James Wigglesworth updated to include Dynamixel Servo Support. See - https://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/ for instructions and connections. - Install Dynamixel libraries into Arduino IDE then "Sketch" / "Include Libraries" / "Manage Libraries" and - then search for Dynamixel. Select and then "Install" the DYNAMIXEL2Arduino and then DynamixelShield. - Set the SERVO_ID, SERVO_MODE, and BAUD defines below as needed (pre-set to defaults). - Note this changes the command serial port and requires a seperate USB/Serial adapter - on the DYNAMIXELShield UART RX/TX connector. -//20200203 James Newton / Tyler Skelton updated to support readback of servo position, torque, and - velocity and to set max "current" (torque(ish)) when moving. -//20211220 James Newton, add support for a step / direction stepper driver with velocity and accel. See -https://www.airspayce.com/mikem/arduino/AccelStepper/index.html for details. Install library via -"Sketch" / "Include Libraries" / "Manage Libraries" and then search for "AccelStep". Select and then "Install" -//20211220 James Newton Clean up code and allow options to NOT support Dynamixel or Steppers via the defines - -A simple Arduino script to set pins high, low, input, pull up, or drive analog / rc servo, -Dynamixel servo, step / direction type stepper motor driver, clock out data with timing, -and read all or a single pin back... all via serial IO. +#include +//unsigned char EEPROM[100]; +//The stupid EEPROM library being included causes all errors to show up as "invalid header file" -Makes the Arduino: -- a tool for generating test signals, and reading back results. -Not as powerful as the busPirate, but more flexible in some ways and much easier to operate. -- a generic IO controller which doesn't need a new program for each application. -Not a replacement for Firmata as this is intended to be used by a human directly -via serial monitor or terminal, not from a program. - -Commands: -#? //return binary value of digital pin, and value for analog input if exists - //if # and default # (set by comma command, see below) are zero or ommitted - //? returns all pins and analog values at once. -#I //set pin # to an input. e.g. 3I -#P //set pin # to an input with internal pullup. 4P -#H //set pin # to a high output. 3H4H -#L //set pin # to a low output. 5L4L3L -#D //delay # microseconds between each command, with a minimum of about 47uS -#, //comma. Saves # as the default for all commands e.g. 3,HLHLHLI -#,#A //set pin # to an analog output with value. Only PWM outputs will respond. - // use with comma command e.g. 5,120A will put 120 on pin 5 -_- //low high clocked puts out the set of low and high signals shown on # with - // a clock on #, e.g. 5,11-__-_--_ clocks out 10010110 on pin 11, with clock - // pulses on pin 5. Clock is currently falling edge only. -. //reads data back from # while clocking #, - // e.g. 5L 11H 5,11-__-_--_. ......... clocks out 10010110, gets the ack, and - // then 8 bits of data and a final ack. -( //I2C start with # as SDA and #, as SCL -) //I2C stop with # as SDA and #, as SCL. Pins left floating pulled up. - // e.g. 5,11(-__-_--_. .........) starts, 10010110, gets ack, data, ack, stop -#,#R //Reboot / Initialize servo into mode. ,R. - // e.g. 1,4R starts servo id 1 in extended position mode -#,#S //Servo position. ,S e.g. 2,90S moves servo id 2 to 90 degrees. -#,#T //Torque setting. ,T e.g. 1,50T sets servo id 1 to half strength. -#,#M //stepper Motor. ,M -#,#V //motion profile for the stepper. , V -#G //goto. Move the stepper motor to the specified position. - -Commands can be strung together on one line; spaces, tabs, carrage returns and line feeds -are all ignored. If no n is specified, value previously saved by , is used. -Examples: -? -//returns something like: {"?":["10010000001111",739,625,569,525,493,470]} -// where 10010000001111 shows the binary value of each pin, from 0 to 14. Pin 0 is first -// 739,625,569,525,493,470 are the values read from each analog channel 0 to 5 -1? -//returns something like: {"1":[1,459]} where 1 is the binary value of pin 1 and -//459 is the analog value of channel 1 -6? -//returns something like {"6":[0]} which is the value of pin 6 (no analog) -4L 6H 5,120A -//(nothing returned) Drives pin 4 low, pin 6 high and puts a PWM / Analog value of 120 on pin 5 -//this also saves pin 5 as the default pin for all commands from now on -240A -//(nothing returned) assuming prior command was 5,120A put 240 out pin 5 as new analog value -? -//assuming 5, has been recieved before, returns just the value of pin 5 and analog 5 -0, -//(nothing returned) clears saved pin, ? now returns all pins. -1000D 5,LHLHLHL -//(nothing returned) delay is 1 millisecond between commands. So pin 5 pulse 3 times at ~200Hz -//Actually about 1.04mS because of the time it takes to recieve and interpret each command. -//The delay command is also useful for making sure all the commands on a line arrive before they are -//excecuted. e.g.: -10000D 3D 5,LHLHL? -//will put out 2 pulses at 50uS per pulse or 10KHz without the 10000D, they are 163uS -//The time it takes to interpret a command is about 47uS so 3D makes it 50. For 100uS -//53D would work. Take the uS delay you want and subtract 47. -//With larger delays, the error is consistant but has relativly less effect. -// Note that the CYCLE_DELAY is not used as long as new characters are available. -1,4R50T90S -//Assuming an attached Dynamixel servo with an ID of 1, reset it to extended position mode, half -// strength, and move to 90 degrees. -*/ +//https://docs.arduino.cc/learn/built-in-libraries/eeprom/ +//EEPROM.clear(); //zero out the entire EEPROM +//EEPROM.length(); //returns the length of the +//byte = EEPROM[addr]; //read ? from EEPROM +//EEPROM[addr] = bytes; //write +//byte = EEPROM.read(addr); //return a single byte out of EEPROM from address +//EEPROM.write(addr, byte); //write a byte in to EEPROM at address +//EEPROM.update(addr, byte); //write a byte in to EEPROM at address, but only if it's not already there +//EEPROM.get(addr, var); //get variable of any size out of EEPROM from address +//EEPROM.put(addr, var); //put variable of any size in to EEPROM at address #define BAUD 115200 //#define DYNAMIXEL_SUPPORT //Note: If enabled, the Dynamixel Arduino shield is required and the standard Arduino UNO serial port is NOT functional! //You must install the USB to Serial interface on the Dynamixel Shield and use that for communication. RTFM -#define STEPPER_SUPPORT - -#include - -#ifdef DYNAMIXEL_SUPPORT -#include - -// Please modify it to suit your hardware. -#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield - #include - SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX - #define DXL_SERIAL Serial - #define DEBUG_SERIAL soft_serial - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield - #define DXL_SERIAL Serial - #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield - #define DXL_SERIAL Serial1 - #define DEBUG_SERIAL SerialUSB - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. - #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) - #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) -#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. - // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. - // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 - #define DXL_SERIAL Serial3 - #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. -#else // Other boards when using DynamixelShield - #define DXL_SERIAL Serial1 - #define DEBUG_SERIAL Serial - const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN -#endif - -#define SERVO_ID 1 - -#define SERVO_MODE OP_EXTENDED_POSITION -//https://emanual.robotis.com/docs/en/popup/arduino_api/setOperatingMode/ +//#define STEPPER_SUPPORT -Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); -int servo_id; -using namespace ControlTableItem; -#else -#define DXL_DIR_PIN -1 -#define DEBUG_SERIAL Serial -#endif - -#ifdef STEPPER_SUPPORT -#include -#include -//the default pins are used if you just start with an M oplet. Or 0,0M -#define DEFAULT_STEP_PIN 3 -#define DEFAULT_DIR_PIN 4 -/* Good starting values for different modes - * Vel Accel Microstep mode - * 30 15 Full - * 60 30 Half - * 120 60 Quarter - * 240 120 Eighth - * 480 240 Sixteenth - */ -#define DEFAULT_VELOCITY 480 -#define DEFAULT_ACCEL 240 -#define DEFAULT_STEP_DELAY_US 1 - -int dir_pin,step_pin; - -void step(){ //TODO Allow for stepper drivers with a negative going step signal. Are there any? - digitalWrite(step_pin, HIGH); // step HIGH - delayMicroseconds(DEFAULT_STEP_DELAY_US); // Delay the minimum allowed pulse width - digitalWrite(step_pin, LOW); // step LOW -} - -void step_forward() { - digitalWrite(dir_pin, HIGH); // Set direction first else get rogue pulses - step(); -} - -void step_back() { - digitalWrite(dir_pin, LOW); // Set direction first else get rogue pulses - step(); -} - -//AccelStepper stepper(AccelStepper::DRIVER, DEFAULT_STEP_PIN, DEFAULT_DIR_PIN); -//Instead, use the version with the two functions, so we can control the pins. -//https://www.airspayce.com/mikem/arduino/AccelStepper/classAccelStepper.html#afa3061ce813303a8f2fa206ee8d012bd -AccelStepper stepper(step_forward, step_back); //avoids setting up the pins now. -#endif #define ANALOG_PINS NUM_ANALOG_INPUTS #define DIGITAL_PINS NUM_DIGITAL_PINS //change above if you want ? to report fewer than actual pins. #define CYCLE_DELAY 100 +#define DXL_DIR_PIN -1 +#define DEBUG_SERIAL Serial + +long n; //number accumulator. New digits are shifted in. 'a'-'z' +long p; //secondary accumulator. ',' shifts n into p. +/* n and p can reference +- just numbers, e.g. degrees for a servo, number of steps to move, etc... +- a pin number, addressing that pin, if the value is between 1 and NUM_DIGITAL_PINS +- a register address, between NUM_DIGITAL_PINS and that plus NO_REGS +- an address in EEPROM, between NUM_DIGITAL_PINS+NO_REGS and that plus EEPROM.length() + +Assuming 20 digital pins, 'a' becomes 20, 'b' 21, etc.. +Digits are shifted into n and p, so '1a' becomes 21 same as 'b' +EEPROM is addressed after 'z' via numbers. '1z' is the first EEPROM address -long n,p,d, radix; +*/ +long d; +long radix; int sign; -char cmd; -char op; -bool quoting; -#define REG_SIZE 26 -long reg[REG_SIZE]; +char cmd; //the current command byte code +char op; //the current operation +bool quoting, debugging; +#define NO_REGS 26 +long reg[NO_REGS]; //registers, each is a long, several bytes +int pc; //program counter. Indexes EEPROM +int stack[5]; //stack +int sp; //stack pointer +int flag; //true false flag + +#define REGLTR(addr) (reg[addr-'a']) +//make it easy to index the reg array with a character letter e.g.REGLTR('b') is reg[1] byte *mem; //will point into reg -/* -https://playground.arduino.cc/Code/PwmFrequency -Pins 9 and 10 run at 31250Hz from Timer1 which is only used for servo. -3 and 11 are on Timer2, 5 and 6 on Timer0. These are also used for delay, millis, etc... -So limiting our changes to Timer1 and pins 9 and 10 allows us to still have timing. - switch(divisor) { - case 1: mode = 0x01; break; //31250Hz - case 8: mode = 0x02; break; //3906.25Hz - case 64: mode = 0x03; break; //488.28125Hz //default? - case 256: mode = 0x04; break; //122.0703125Hz - case 1024: mode = 0x05; break;//30.517578125Hz - } - TCCR1B = TCCR1B & 0b11111000 | mode; -https://www.arduino.cc/en/Tutorial/SecretsOfArduinoPWM -Says only Timer0 is used for delay and millis... -We can try changing 3 and 11 on Timer2 -Different divisors, so compensate -T2 T1 / Freq -0x01 1 1 31250 -0x02 2 8 3906.25 -0x03 2 32 976.5625 -0x04 3 64 488.28125 //default? -0x05 3 128 244.140625 -0x06 4 256 122.0703125 -0x07 5 1024 30.517578125 -TCCR2B = TCCR2B & 0b11111000 | mode; -if (n>2) n--; //1,2,2,3,4,5,6 -if (n>3) n--; //1,2,2,3,3,4,5 -formula is f = clock / (510 * mode) where clock=16MHz -If you mess with TCCR0B, delay can be compensated as follows -0x01: delay(64000) or 64000 millis() ~ 1 second -0x02: delay(8000) or 8000 millis() ~ 1 second -0x03: delay(1000) or 1000 millis() ~ 1 second //default -0x04: delay(250) or 250 millis() ~ 1 second -0x05: delay(62) or 62 millis() ~ 1 second -(Or 63 if you need to round up. The number is actually 62.5) -void setPin9_10PWMFreq(freq) { - -} -*/ + void delayus(unsigned long us) { if (us>10000) { //can't delayMicroseconds() more than 16838 @@ -254,96 +67,133 @@ void delayus(unsigned long us) { delayMicroseconds(us); } -#ifdef DYNAMIXEL_SUPPORT -void rebootServo(int id, int mode) { //setup servo id number into mode. - dxl.torqueOff(id); - if (!mode) mode = SERVO_MODE; - DEBUG_SERIAL.print("[{\"Servo\": "); - DEBUG_SERIAL.print(id); - if (dxl.setOperatingMode(id, mode)){ - DEBUG_SERIAL.print(", \"Mode\": "); - DEBUG_SERIAL.print(mode); - } - if (dxl.writeControlTableItem(PROFILE_VELOCITY, id, 0)){ //0 is no velocity limit - DEBUG_SERIAL.print(", \"Velocity\": \"MAX\""); - } - else { //can't set max velocity! - DEBUG_SERIAL.print(", \"Velocity\": \"ERR\""); - } - if (dxl.torqueOn(id)){ - DEBUG_SERIAL.println(", \"Torque\": \"On\""); - } - DEBUG_SERIAL.println("}]"); - } -#endif void setup() { DEBUG_SERIAL.begin(BAUD); while(!DEBUG_SERIAL); //Wait until the serial port is opened - DEBUG_SERIAL.println("[{\"Ready\": \"true\"}]"); -#ifdef DYNAMIXEL_SUPPORT - servo_id = SERVO_ID; - dxl.setPortProtocolVersion(2.0); - dxl.begin(BAUD); - //rebootServo(); -#endif + n=0; //number p=0; //pin number d=2; //delay. Default is 2uS or 250KHz radix=10; sign=1; - mem = reg[0]; - EEPROM.get(0, reg['m'-'a']); //load up the count of memory used + //mem = reg[0]; + EEPROM.get(0,REGLTR('m'));//load up the count of memory used + REGLTR('m')+=sizeof(reg[0]); op = 0; - quoting = false; -#ifdef STEPPER_SUPPORT - stepper.setAcceleration(DEFAULT_ACCEL);stepper.setMaxSpeed(DEFAULT_VELOCITY); - dir_pin = DEFAULT_DIR_PIN; - step_pin = DEFAULT_STEP_PIN; -#endif + pc = 0; + sp = 0; + debugging = true; + } + +int printstat() { + DEBUG_SERIAL.print("\n"); + DEBUG_SERIAL.print(p); + DEBUG_SERIAL.print(","); + DEBUG_SERIAL.print(n); + + DEBUG_SERIAL.print(" R:"); + for (int i=0; i < 3; i++) { + DEBUG_SERIAL.print(reg[i]); + DEBUG_SERIAL.print(", "); + } + + DEBUG_SERIAL.print(" S:"); + for (int i=0; i < 5; i++) { + DEBUG_SERIAL.print(stack[i]); + DEBUG_SERIAL.print(", "); + } + + DEBUG_SERIAL.print(" E:"); + for (int i=sizeof(reg[0]); i < REGLTR('m'); i++) { + DEBUG_SERIAL.print((char)EEPROM[i]); } + DEBUG_SERIAL.print(" #"); + DEBUG_SERIAL.println(REGLTR('m')); + return 1; +} + +int printreg(int n) { + DEBUG_SERIAL.print((char)('a'+n)); + DEBUG_SERIAL.print(":"); + char c; + for (int i=reg[n]; i < REGLTR('m'); i++) { + c = (char)EEPROM[i]; + DEBUG_SERIAL.print(c); + if ('.' == c) break; + } + DEBUG_SERIAL.print('\n'); +} void loop(){ - while (DEBUG_SERIAL.available() > 0) { //if data has arrived - int c = DEBUG_SERIAL.read(); //get the data + //use this while loop so we can "continue" from anywhere when we finish with a new character + while ((DEBUG_SERIAL.available()) //if data has arrived, + || (pc > 0)//or we are running from EEPROM + ) { + int c = pc?(char)EEPROM.read(pc++):DEBUG_SERIAL.read(); //get the data cmd = char(c); + if (debugging) DEBUG_SERIAL.print(cmd); if ('\"'==cmd) { //quote if (!quoting) { //this is a new quote - if (NUM_DIGITAL_PINS >= n || REG_SIZE+NUM_DIGITAL_PINS <= n) {DEBUG_SERIAL.print("reg?");continue;} - reg[n-NUM_DIGITAL_PINS]=reg['m'-'a']; //reg being defined points to mem at reg m + //if (NUM_DIGITAL_PINS >= n || REG_SIZE+NUM_DIGITAL_PINS <= n) {DEBUG_SERIAL.print("reg?");} + reg[n-NUM_DIGITAL_PINS]=REGLTR('m'); //reg being defined points to mem at reg m quoting = true; } else { //end of a quote - EEPROM.put(0, reg['m'-'a']); //save the number of bytes we've used + EEPROM.put(EEPROM.length()-sizeof(reg[0]), REGLTR('m')); //save the number of bytes we've used quoting = false; + if (debugging) printreg(n-DIGITAL_PINS); } continue; //in either case, we are done here } - if ('\n'==cmd || '\r'==cmd) { //EOL - if (NUM_DIGITAL_PINS < p && REG_SIZE+NUM_DIGITAL_PINS > p) { //p is a register - reg[p-NUM_DIGITAL_PINS]=n; //TODO check for an ':' op as well. actually do a switch here. + if (quoting) { //if we are quoting, just put it away + EEPROM.update(REGLTR('m'),cmd); //add the current address, then increment it + if (debugging) { + DEBUG_SERIAL.print(REGLTR('m')); + DEBUG_SERIAL.print(':'); + DEBUG_SERIAL.println((char)EEPROM.read(REGLTR('m'))); + } + REGLTR('m')++; + continue; //and do nothing more + } + if ('\n'==cmd || '\r'==cmd || '.'==cmd || 0==cmd) { //EOL + if (DIGITAL_PINS < p && NO_REGS+DIGITAL_PINS > p) { //p is a register + reg[p-DIGITAL_PINS]=n; //TODO check for an ':' op as well. actually do a switch here. p=0; //for now } n=0; cmd=0;//clear command and value (but not p) at end of line. d=0; //delay doesn't last past one line - quoting = false; //no multi-line quoting, for now... maybe later + pc = stack[sp]; //return, always a zero at stack[0] + if (sp) sp--; //stop at 0 + quoting = false; + DEBUG_SERIAL.print("cr"); continue; //loop now, no delay } - if (quoting) { //if we are quoting, just put it away - DEBUG_SERIAL.print(reg['m'-'a']); - EEPROM.update(reg['m'-'a']++,cmd); //add the current address, then increment it - DEBUG_SERIAL.print(':'); - DEBUG_SERIAL.println(cmd); - continue; //and do nothing more - } - if ('@'==cmd && n < EEPROM.length()) { //value AT address - n=EEPROM.read(n); //Get the value out of memory at that address + if (')'==cmd) {//call + if (sp >= sizeof(stack)/sizeof(stack[0])) { + DEBUG_SERIAL.println("\nSTACK OVERFLOW"); + continue; + } + sp++; stack[sp] = pc; + pc = reg[n-NUM_DIGITAL_PINS]; + n = 0; + if (debugging) {DEBUG_SERIAL.print("run from "); DEBUG_SERIAL.println(pc);} + continue; + } + if ('@'==cmd) {// //value AT address, source based on range + if (n < ANALOG_PINS) { n = analogRead(n); } + else if (n < DIGITAL_PINS) { n = digitalRead(n); } + else if (n < DIGITAL_PINS+NO_REGS ) { n = reg[n-DIGITAL_PINS]; } + else if (n >= DIGITAL_PINS+NO_REGS ) { n = EEPROM[n-(DIGITAL_PINS+NO_REGS)];} continue; } - if ('!'==cmd && p < EEPROM.length()) { //Write value to address - EEPROM.update(p,n); //put n into EEPROM at p + if ('!'==cmd) {// Write value to address, destination depends on range + if (p < ANALOG_PINS) { analogWrite(p, n); } + else if (p < DIGITAL_PINS) { digitalWrite(p, n); } + else if (p < DIGITAL_PINS+NO_REGS ) { reg[p-DIGITAL_PINS] = n; } + else if (p >= DIGITAL_PINS+NO_REGS ) { EEPROM[p-(DIGITAL_PINS+NO_REGS)]=n;} continue; } - if ('0' <= c && c <= '9') { //if it's a digit +if ('0' <= c && c <= '9') { //if it's a digit n = (c-'0')*sign + n*radix; //add it to n, shift n up 1 digit sign = 1; //in case it was negative continue; //and loop @@ -378,15 +228,8 @@ void loop(){ DEBUG_SERIAL.print(","); DEBUG_SERIAL.print(analogRead(p)); } - } -#ifdef DYNAMIXEL_SUPPORT - DEBUG_SERIAL.print(","); //add in data about the first servo. - DEBUG_SERIAL.print(dxl.getPresentPosition(servo_id, UNIT_DEGREE)); - DEBUG_SERIAL.print(","); - DEBUG_SERIAL.print(dxl.getPresentPWM(servo_id, UNIT_PERCENT)); - DEBUG_SERIAL.print(","); - DEBUG_SERIAL.print(dxl.getPresentVelocity(servo_id, UNIT_RPM)); -#endif + } + DEBUG_SERIAL.print("]"); } else { //specific pin or address DEBUG_SERIAL.print("\""); @@ -397,7 +240,7 @@ void loop(){ if (NUM_DIGITAL_PINS > n) { //it's a pin value = digitalRead(n); } else { //it's an address - if (REG_SIZE+NUM_DIGITAL_PINS < n) n = REG_SIZE+NUM_DIGITAL_PINS; + if (NO_REGS+NUM_DIGITAL_PINS < n) n = NO_REGS+NUM_DIGITAL_PINS; value = reg[n-NUM_DIGITAL_PINS]; //to a register } DEBUG_SERIAL.print(value); //just that one value @@ -407,17 +250,9 @@ void loop(){ DEBUG_SERIAL.print(value); //also return it } } -#ifdef DYNAMIXEL_SUPPORT - else { //servo data - DEBUG_SERIAL.print(dxl.getPresentPosition(p, UNIT_DEGREE)); - DEBUG_SERIAL.print(","); - DEBUG_SERIAL.print(dxl.getPresentPWM(p, UNIT_PERCENT)); - DEBUG_SERIAL.print(","); - DEBUG_SERIAL.print(dxl.getPresentVelocity(p, UNIT_RPM)); - } -#endif + DEBUG_SERIAL.print("]"); } - DEBUG_SERIAL.println("]}"); + DEBUG_SERIAL.println("}"); //DEBUG_SERIAL.write(04); //EOT //sending EOT can help OS serial device drivers return data instead of waiting forever for the file to end. //https://stackoverflow.com/questions/50178789/signal-end-of-file-in-serial-communication @@ -471,77 +306,14 @@ void loop(){ case 'D': //delay n ms per instruction d=n; break; -#ifdef STEPPER_SUPPORT - case 'M': //,M step, direction Motor stepper - if (p) step_pin = p; - if (n) dir_pin = n; - DEBUG_SERIAL.print("[{\"Step\": "); - DEBUG_SERIAL.print(step_pin); - DEBUG_SERIAL.print(", \"Dir\": "); - DEBUG_SERIAL.print(dir_pin); - DEBUG_SERIAL.println("}]"); - pinMode(step_pin, OUTPUT);digitalWrite(step_pin, LOW); - pinMode(dir_pin, OUTPUT);digitalWrite(dir_pin, LOW); - p = n = 0; - //break; //fall through and set default accel and velocity - case 'V': //, V Set acceleration and velocity - //,V Set default acceleration and velocity - if (!p) p = DEFAULT_ACCEL; - if (!n) n = DEFAULT_VELOCITY; - stepper.setAcceleration(p + 1);stepper.setMaxSpeed(n); - DEBUG_SERIAL.print("[{\"Accelleration\": "); - DEBUG_SERIAL.print(p+1); - DEBUG_SERIAL.print(", \"Velocity\": "); - DEBUG_SERIAL.print(n); - DEBUG_SERIAL.println("}]"); - break; - case 'G': //G position Goto - stepper.moveTo(n); - DEBUG_SERIAL.print("[{\"Goto\": "); - DEBUG_SERIAL.print(n); - DEBUG_SERIAL.println("}]"); - break; -/* -#E pin Endstop. Input with Pullup. Run motor ccw until pin goes low. stepper.setMaxSpeed(SLOW_SPEED); stepper.moveTo(-MAX_LONG); if (!digitalRead(n)) stepper.stop(); - */ -#endif -#ifdef DYNAMIXEL_SUPPORT - case 'R': //,S reboots servo. - servo_id = p; - rebootServo(p,n); - break; - case 'S': //, sets goal position. e.g. 1,90S 100000D 45S - if (dxl.setGoalPosition(p, n, UNIT_DEGREE)) { - DEBUG_SERIAL.print("[{\"Servo\": "); - DEBUG_SERIAL.print(p); - DEBUG_SERIAL.print(", \"Goal\": "); - DEBUG_SERIAL.print(n); - DEBUG_SERIAL.println("}]"); - } - else { //can't move! - DEBUG_SERIAL.println("[{\"Error:\" \"ServoPosition\"}]"); - } - break; - case 'T': - if (dxl.setGoalPWM(p, n, UNIT_PERCENT)) { - DEBUG_SERIAL.print("[{\"Servo\": "); - DEBUG_SERIAL.print(p); - DEBUG_SERIAL.print(", \"Torque\": "); - DEBUG_SERIAL.print(n); - DEBUG_SERIAL.println("}]"); - } - else { //can't set torque! - DEBUG_SERIAL.println("[{\"Error:\" \"ServoTorque\"}]"); - } - break; -#endif + default: DEBUG_SERIAL.print("\""); DEBUG_SERIAL.print(n); DEBUG_SERIAL.print(cmd); DEBUG_SERIAL.println("?\""); - } + } if ('0'>cmd || '_'==cmd) {//was it punctuation? digitalWrite(p,HIGH); //raise the clock pinMode(p,OUTPUT); @@ -556,10 +328,12 @@ void loop(){ } //p is NOT cleared, so you can keep sending new commands only cmd=0; //done with command. - } -#ifdef STEPPER_SUPPORT - stepper.run(); //run full speed to support stepping. -#else + } delayus(CYCLE_DELAY); //save a little energy if we don't have characters. -#endif + if (debugging) { + printstat(); + while (!DEBUG_SERIAL.available()) ; } + + +} From fbaefda49b7ee7ec3a0cdbcc6de68072263fc0e0 Mon Sep 17 00:00:00 2001 From: JamesNewton Date: Thu, 20 Jun 2024 21:19:44 -0700 Subject: [PATCH 3/5] Conditionals working - Check for and initialized EEPROM if first run or invalid data. - Limit program space in EEPROM for chips with large memory. - printreg displays the value of the register as well as EEPROM at its address. - moved existing "?" display code into display_status - only '.' does a return. cr just ends lines / - move commands into switch: ')', '@', '!' - no longer copy p into n if n is 0 and no longer zero out n after every command. Not sure why we did that, other than to keep a pin number from one line to the next. But we can do that in the pin control command? - Give up on I2C commands to free up ')' and '.' which also simplifies '-' and frees up '(' and '_'. Eventually '(' is for parameters. Maybe use '_' to clear when cr isn't available? Hopefully I2C can be implemented in the actual langauge. Or as a mode change / sub engine. --- Arduino_Dynamixel_Controller.ino | 231 +++++++++++++++---------------- 1 file changed, 109 insertions(+), 122 deletions(-) diff --git a/Arduino_Dynamixel_Controller.ino b/Arduino_Dynamixel_Controller.ino index 9499579..cff78c2 100644 --- a/Arduino_Dynamixel_Controller.ino +++ b/Arduino_Dynamixel_Controller.ino @@ -3,7 +3,6 @@ //The stupid EEPROM library being included causes all errors to show up as "invalid header file" //https://docs.arduino.cc/learn/built-in-libraries/eeprom/ -//EEPROM.clear(); //zero out the entire EEPROM //EEPROM.length(); //returns the length of the //byte = EEPROM[addr]; //read ? from EEPROM //EEPROM[addr] = bytes; //write @@ -19,7 +18,7 @@ //You must install the USB to Serial interface on the Dynamixel Shield and use that for communication. RTFM //#define STEPPER_SUPPORT - +#define PROG_SPACE 100 #define ANALOG_PINS NUM_ANALOG_INPUTS #define DIGITAL_PINS NUM_DIGITAL_PINS //change above if you want ? to report fewer than actual pins. @@ -51,7 +50,7 @@ long reg[NO_REGS]; //registers, each is a long, several bytes int pc; //program counter. Indexes EEPROM int stack[5]; //stack int sp; //stack pointer -int flag; //true false flag +bool skipping; //are we skipping for a conditional #define REGLTR(addr) (reg[addr-'a']) //make it easy to index the reg array with a character letter e.g.REGLTR('b') is reg[1] @@ -71,6 +70,18 @@ void delayus(unsigned long us) { void setup() { DEBUG_SERIAL.begin(BAUD); while(!DEBUG_SERIAL); //Wait until the serial port is opened + int l = EEPROM.length(); + if (l>PROG_SPACE) l = PROG_SPACE; + EEPROM.get(0,n);//load up the count of memory used + if (n<=0 || n>l) { + for (int i = 0; i n) { //it's a pin + value = digitalRead(n); + } else { //it's an address + if (NO_REGS+NUM_DIGITAL_PINS < n) n = NO_REGS+NUM_DIGITAL_PINS; + value = reg[n-NUM_DIGITAL_PINS]; //to a register + } + DEBUG_SERIAL.print(value); //just that one value + if (ANALOG_PINS > n) { //if there is an analog channel + value = analogRead(p); //use that instead. + DEBUG_SERIAL.print(","); + DEBUG_SERIAL.print(value); //also return it + } + } + DEBUG_SERIAL.print("]"); + } + DEBUG_SERIAL.println("}"); + //DEBUG_SERIAL.write(04); //EOT + //sending EOT can help OS serial device drivers return data instead of waiting forever for the file to end. + //https://stackoverflow.com/questions/50178789/signal-end-of-file-in-serial-communication + DEBUG_SERIAL.print("\n"); //new line can help after EOT to tigger xmit on OS serial handler + + } void loop(){ @@ -147,11 +208,6 @@ void loop(){ } if (quoting) { //if we are quoting, just put it away EEPROM.update(REGLTR('m'),cmd); //add the current address, then increment it - if (debugging) { - DEBUG_SERIAL.print(REGLTR('m')); - DEBUG_SERIAL.print(':'); - DEBUG_SERIAL.println((char)EEPROM.read(REGLTR('m'))); - } REGLTR('m')++; continue; //and do nothing more } @@ -160,40 +216,19 @@ void loop(){ reg[p-DIGITAL_PINS]=n; //TODO check for an ':' op as well. actually do a switch here. p=0; //for now } + if ('.'==cmd || 0==cmd) { + pc = stack[sp]; //return, always a zero at stack[0] + if (sp) sp--; //stop at 0 + } n=0; cmd=0;//clear command and value (but not p) at end of line. d=0; //delay doesn't last past one line - pc = stack[sp]; //return, always a zero at stack[0] - if (sp) sp--; //stop at 0 quoting = false; + skipping = false; //only skip to the end of the line. DEBUG_SERIAL.print("cr"); continue; //loop now, no delay } - if (')'==cmd) {//call - if (sp >= sizeof(stack)/sizeof(stack[0])) { - DEBUG_SERIAL.println("\nSTACK OVERFLOW"); - continue; - } - sp++; stack[sp] = pc; - pc = reg[n-NUM_DIGITAL_PINS]; - n = 0; - if (debugging) {DEBUG_SERIAL.print("run from "); DEBUG_SERIAL.println(pc);} - continue; - } - if ('@'==cmd) {// //value AT address, source based on range - if (n < ANALOG_PINS) { n = analogRead(n); } - else if (n < DIGITAL_PINS) { n = digitalRead(n); } - else if (n < DIGITAL_PINS+NO_REGS ) { n = reg[n-DIGITAL_PINS]; } - else if (n >= DIGITAL_PINS+NO_REGS ) { n = EEPROM[n-(DIGITAL_PINS+NO_REGS)];} - continue; - } - if ('!'==cmd) {// Write value to address, destination depends on range - if (p < ANALOG_PINS) { analogWrite(p, n); } - else if (p < DIGITAL_PINS) { digitalWrite(p, n); } - else if (p < DIGITAL_PINS+NO_REGS ) { reg[p-DIGITAL_PINS] = n; } - else if (p >= DIGITAL_PINS+NO_REGS ) { EEPROM[p-(DIGITAL_PINS+NO_REGS)]=n;} - continue; - } -if ('0' <= c && c <= '9') { //if it's a digit + if (skipping) continue; //unless it was an EOL, it's after a failed '?', so we don't care. + if ('0' <= c && c <= '9') { //if it's a digit n = (c-'0')*sign + n*radix; //add it to n, shift n up 1 digit sign = 1; //in case it was negative continue; //and loop @@ -207,63 +242,49 @@ if ('0' <= c && c <= '9') { //if it's a digit if (','==cmd) { p=n; n=0; continue;} //save n to p, clear n, loop if (0==n) { if ('-'==cmd) {sign = -1; continue;} //if we don't have a number, it's a negative + /* this seems like a super bad idea... why do this? n=p; //if we don't have a value, use the prevous pin number. //Note this means n can't be zero unless p is. 1,0 isn't possible, it becomes 1,1 + */ } switch (cmd) { + case ')': //call + if (sp >= sizeof(stack)/sizeof(stack[0])) { + DEBUG_SERIAL.println("\nSTACK OVERFLOW"); + break; + } + sp++; stack[sp] = pc; + pc = reg[n-NUM_DIGITAL_PINS]; + n = 0; + if (debugging) {DEBUG_SERIAL.print("run from "); DEBUG_SERIAL.println(pc);} + break; + case '@': // //value AT address, source based on range + if (n < ANALOG_PINS) { n = analogRead(n); } + else if (n < DIGITAL_PINS) { n = digitalRead(n); } + else if (n < DIGITAL_PINS+NO_REGS ) { n = reg[n-DIGITAL_PINS]; } + else if (n >= DIGITAL_PINS+NO_REGS ) { n = EEPROM[n-(DIGITAL_PINS+NO_REGS)];} + break; + case '!': // Write value to address, destination depends on range + if (p < ANALOG_PINS) { analogWrite(p, n); } + else if (p < DIGITAL_PINS) { digitalWrite(p, n); } + else if (p < DIGITAL_PINS+NO_REGS ) { reg[p-DIGITAL_PINS] = n; } + else if (p >= DIGITAL_PINS+NO_REGS ) { EEPROM[p-(DIGITAL_PINS+NO_REGS)]=n;} + break; case '?': //get information - DEBUG_SERIAL.print("{"); //optional, just to signal start of data - if (0==n) { //if we didn't have a number selecting a pin - DEBUG_SERIAL.print("\"?\":[\""); //optional, just to signal start of data - for (int p = 0; p < DIGITAL_PINS; p++) { //get all the pins - //n = digitalRead(p) + n<<1; //convert to binary number - if (DXL_DIR_PIN != p) { //don't mess with the servo pins - DEBUG_SERIAL.print(digitalRead(p));//and also print. - } - } - DEBUG_SERIAL.print("\""); - //DEBUG_SERIAL.print(n); //print the binary value of all pins - for (int p = 0; p < ANALOG_PINS; p++) { //also check all the analog - if (DXL_DIR_PIN != p) { //don't mess with the servo pins - DEBUG_SERIAL.print(","); - DEBUG_SERIAL.print(analogRead(p)); - } + if (pc) { //executing + if (!n) {//and not true + skipping = true; } - DEBUG_SERIAL.print("]"); - } - else { //specific pin or address - DEBUG_SERIAL.print("\""); - DEBUG_SERIAL.print(n); - DEBUG_SERIAL.print("\":["); - if (DXL_DIR_PIN != n) { //don't mess with the servo pins - int value = 0; - if (NUM_DIGITAL_PINS > n) { //it's a pin - value = digitalRead(n); - } else { //it's an address - if (NO_REGS+NUM_DIGITAL_PINS < n) n = NO_REGS+NUM_DIGITAL_PINS; - value = reg[n-NUM_DIGITAL_PINS]; //to a register - } - DEBUG_SERIAL.print(value); //just that one value - if (ANALOG_PINS > n) { //if there is an analog channel - value = analogRead(p); //use that instead. - DEBUG_SERIAL.print(","); - DEBUG_SERIAL.print(value); //also return it - } - } - DEBUG_SERIAL.print("]"); + n = 0; //done with the condition, clear for the next command. } - DEBUG_SERIAL.println("}"); - //DEBUG_SERIAL.write(04); //EOT -//sending EOT can help OS serial device drivers return data instead of waiting forever for the file to end. -//https://stackoverflow.com/questions/50178789/signal-end-of-file-in-serial-communication - DEBUG_SERIAL.print("\n"); //new line can help after EOT to tigger xmit on OS serial handler + else { //interactive + display_status(); + } break; - case '-': case 'H': //set pin n output high pinMode(n,OUTPUT); digitalWrite(n,HIGH); break; - case '_': case 'L': //set pin n output low pinMode(n,OUTPUT); digitalWrite(n,LOW); @@ -274,31 +295,6 @@ if ('0' <= c && c <= '9') { //if it's a digit case 'P': //set pin n input with pullup pinMode(n,INPUT_PULLUP); break; - case '.': //clock in data from n via p - pinMode(n,INPUT_PULLUP); //make n input with pull now - break; - case '(': //I2C start, data low while clock high - pinMode(n,OUTPUT); - digitalWrite(n,HIGH); //data high - pinMode(p,OUTPUT); //setup clock (if not already) - digitalWrite(p,HIGH); //send clock high - delayus(d); //wait - digitalWrite(n,LOW); //data low - delayus(d); //wait - digitalWrite(p,LOW); //send clock low - continue; //no further processing - break; - case ')': //I2C stop, data low while clock high - pinMode(n,OUTPUT); //data may be floating high (input from slave) - digitalWrite(n,LOW); //so we need to drive it low - pinMode(p,OUTPUT); //setup clock (if not already) - digitalWrite(p,LOW); //send clock high - delayus(d); - pinMode(p,INPUT_PULLUP); //clock floats high - delayus(d); - pinMode(n,INPUT_PULLUP); //data floats high - continue; //no further processing - break; case 'A': //set pin p to analog output value n pinMode(p,OUTPUT); analogWrite(p,n); @@ -314,22 +310,12 @@ if ('0' <= c && c <= '9') { //if it's a digit DEBUG_SERIAL.print(cmd); DEBUG_SERIAL.println("?\""); } - if ('0'>cmd || '_'==cmd) {//was it punctuation? - digitalWrite(p,HIGH); //raise the clock - pinMode(p,OUTPUT); - delayus(d/2); //half delay - if ('.'==cmd) {DEBUG_SERIAL.print(digitalRead(n));} - digitalWrite(p,LOW); //drop the clock - delayus(d/2); //half delay - } - else { - n=0; //zero out value for next command. - delayus(d); //wait a bit for the next cycle. - } + //n=0; //zero out value for next command. No, only at end of line. + //delayus(d); //wait a bit for the next cycle. //p is NOT cleared, so you can keep sending new commands only cmd=0; //done with command. } - delayus(CYCLE_DELAY); //save a little energy if we don't have characters. + //delayus(CYCLE_DELAY); //save a little energy if we don't have characters. if (debugging) { printstat(); while (!DEBUG_SERIAL.available()) ; @@ -337,3 +323,4 @@ if ('0' <= c && c <= '9') { //if it's a digit } + From 6e338ad190da9ca65dd5c8a5b4b1ed1e491e69de Mon Sep 17 00:00:00 2001 From: JamesNewton Date: Sun, 7 Jul 2024 22:06:31 -0700 Subject: [PATCH 4/5] I and P now replace n with pin or register value Digital ONLY pins are now differentiated from analog pins. e.g. on Arduino Uno, 2 to 13 are digital only, 14 to 19 are analog (or digital) but we read them via analog. This allows I and P to be used to read values inside a program, since ? is a conditional branch. e.g. 2I will read pin 2 and set n to 0 or 1. 14I will read pin 14 aka A0 and set n to the analog value read on the pin. --- Arduino_Dynamixel_Controller.ino | 83 ++++++++++++++++++++++---------- 1 file changed, 57 insertions(+), 26 deletions(-) diff --git a/Arduino_Dynamixel_Controller.ino b/Arduino_Dynamixel_Controller.ino index cff78c2..04081c9 100644 --- a/Arduino_Dynamixel_Controller.ino +++ b/Arduino_Dynamixel_Controller.ino @@ -19,9 +19,15 @@ //#define STEPPER_SUPPORT #define PROG_SPACE 100 +//count of pins which support analog modes #define ANALOG_PINS NUM_ANALOG_INPUTS -#define DIGITAL_PINS NUM_DIGITAL_PINS -//change above if you want ? to report fewer than actual pins. +//Turns out NUM_DIGITAL_PINS counts ALL pins which can be used digitally, +//including the analog pins +#define NUM_PINS NUM_DIGITAL_PINS +//Track the digital /only/ pins. +#define DIGITAL_PINS (NUM_DIGITAL_PINS - ANALOG_PINS) +//The first analog pin number is the last digital only pin + 1 +//change above if you want to use fewer than actual pins. #define CYCLE_DELAY 100 #define DXL_DIR_PIN -1 #define DEBUG_SERIAL Serial @@ -30,9 +36,9 @@ long n; //number accumulator. New digits are shifted in. 'a'-'z' long p; //secondary accumulator. ',' shifts n into p. /* n and p can reference - just numbers, e.g. degrees for a servo, number of steps to move, etc... -- a pin number, addressing that pin, if the value is between 1 and NUM_DIGITAL_PINS -- a register address, between NUM_DIGITAL_PINS and that plus NO_REGS -- an address in EEPROM, between NUM_DIGITAL_PINS+NO_REGS and that plus EEPROM.length() +- a pin number, addressing that pin, if the value is between 1 and NUM_PINS +- a register address, between NUM_PINS and that plus NO_REGS +- an address in EEPROM, between NUM_PINS+NO_REGS and that plus EEPROM.length() Assuming 20 digital pins, 'a' becomes 20, 'b' 21, etc.. Digits are shifted into n and p, so '1a' becomes 21 same as 'b' @@ -93,6 +99,12 @@ void setup() { pc = 0; sp = 0; debugging = true; + DEBUG_SERIAL.print(NUM_PINS); + DEBUG_SERIAL.print(" pins, "); + DEBUG_SERIAL.print(DIGITAL_PINS); + DEBUG_SERIAL.print(" digital, "); + DEBUG_SERIAL.print(ANALOG_PINS); + DEBUG_SERIAL.print(" analog. "); } int printstat() { @@ -122,7 +134,7 @@ int printstat() { return 1; } -int printreg(int n) { +void printreg(int n) { DEBUG_SERIAL.print((char)('a'+n)); DEBUG_SERIAL.print(":"); DEBUG_SERIAL.print(reg[n]); @@ -140,7 +152,7 @@ void display_status() { DEBUG_SERIAL.print("{"); //optional, just to signal start of data if (0==n) { //if we didn't have a number selecting a pin DEBUG_SERIAL.print("\"?\":[\""); //optional, just to signal start of data - for (int p = 0; p < DIGITAL_PINS; p++) { //get all the pins + for (int p = 0; p < NUM_PINS; p++) { //get all the pins //n = digitalRead(p) + n<<1; //convert to binary number if (DXL_DIR_PIN != p) { //don't mess with the servo pins DEBUG_SERIAL.print(digitalRead(p));//and also print. @@ -162,15 +174,15 @@ void display_status() { DEBUG_SERIAL.print("\":["); if (DXL_DIR_PIN != n) { //don't mess with the servo pins int value = 0; - if (NUM_DIGITAL_PINS > n) { //it's a pin + if (NUM_PINS > n) { //it's a pin value = digitalRead(n); } else { //it's an address - if (NO_REGS+NUM_DIGITAL_PINS < n) n = NO_REGS+NUM_DIGITAL_PINS; - value = reg[n-NUM_DIGITAL_PINS]; //to a register + if (NO_REGS+NUM_PINS < n) n = NO_REGS+NUM_PINS; + value = reg[n-NUM_PINS]; //to a register } DEBUG_SERIAL.print(value); //just that one value - if (ANALOG_PINS > n) { //if there is an analog channel - value = analogRead(p); //use that instead. + if (n > DIGITAL_PINS && n < NUM_PINS) { //if there is an analog channel + value = analogRead(n); //use that as well. DEBUG_SERIAL.print(","); DEBUG_SERIAL.print(value); //also return it } @@ -186,6 +198,21 @@ void display_status() { } +/* test code + +a"13L_b@?13H.". +b,1! +a) //verify led on +b,0! +a) //verify led off + +a"13L_2I?13H.". +//set pin 2 to 0 +a) //verify no led +//set pin 2 to 1 +a) //verify led on + +*/ void loop(){ //use this while loop so we can "continue" from anywhere when we finish with a new character while ((DEBUG_SERIAL.available()) //if data has arrived, @@ -196,13 +223,13 @@ void loop(){ if (debugging) DEBUG_SERIAL.print(cmd); if ('\"'==cmd) { //quote if (!quoting) { //this is a new quote - //if (NUM_DIGITAL_PINS >= n || REG_SIZE+NUM_DIGITAL_PINS <= n) {DEBUG_SERIAL.print("reg?");} - reg[n-NUM_DIGITAL_PINS]=REGLTR('m'); //reg being defined points to mem at reg m + //if (NUM_PINS >= n || REG_SIZE+NUM_PINS <= n) {DEBUG_SERIAL.print("reg?");} + reg[n-NUM_PINS]=REGLTR('m'); //reg being defined points to mem at reg m quoting = true; } else { //end of a quote EEPROM.put(EEPROM.length()-sizeof(reg[0]), REGLTR('m')); //save the number of bytes we've used quoting = false; - if (debugging) printreg(n-DIGITAL_PINS); + if (debugging) printreg(n-NUM_PINS); } continue; //in either case, we are done here } @@ -211,9 +238,9 @@ void loop(){ REGLTR('m')++; continue; //and do nothing more } - if ('\n'==cmd || '\r'==cmd || '.'==cmd || 0==cmd) { //EOL - if (DIGITAL_PINS < p && NO_REGS+DIGITAL_PINS > p) { //p is a register - reg[p-DIGITAL_PINS]=n; //TODO check for an ':' op as well. actually do a switch here. + if ('\n'==cmd || '\r'==cmd || '_'==cmd || '.'==cmd || 0==cmd) { //EOL + if (NUM_PINS < p && NO_REGS+NUM_PINS > p) { //p is a register + reg[p-NUM_PINS]=n; //TODO check for an ':' op as well. actually do a switch here. p=0; //for now } if ('.'==cmd || 0==cmd) { @@ -234,7 +261,7 @@ void loop(){ continue; //and loop } if ('a' <= cmd && cmd <= 'z') { //if it's a variable - n = n + (c - 'a') + NUM_DIGITAL_PINS; //store the address + n = n + (c - 'a') + NUM_PINS; //store the address //note that n is folded in. e.g. 1a is b continue; //and loop } @@ -254,21 +281,21 @@ void loop(){ break; } sp++; stack[sp] = pc; - pc = reg[n-NUM_DIGITAL_PINS]; + pc = reg[n-NUM_PINS]; n = 0; if (debugging) {DEBUG_SERIAL.print("run from "); DEBUG_SERIAL.println(pc);} break; case '@': // //value AT address, source based on range - if (n < ANALOG_PINS) { n = analogRead(n); } + if (n > DIGITAL_PINS && n < NUM_PINS) { n = analogRead(n); } else if (n < DIGITAL_PINS) { n = digitalRead(n); } - else if (n < DIGITAL_PINS+NO_REGS ) { n = reg[n-DIGITAL_PINS]; } - else if (n >= DIGITAL_PINS+NO_REGS ) { n = EEPROM[n-(DIGITAL_PINS+NO_REGS)];} + else if (n < NUM_PINS+NO_REGS ) { n = reg[n-NUM_PINS]; } + else if (n >= NUM_PINS+NO_REGS ) { n = EEPROM[n-(NUM_PINS+NO_REGS)];} break; case '!': // Write value to address, destination depends on range - if (p < ANALOG_PINS) { analogWrite(p, n); } + if (p > DIGITAL_PINS && p < NUM_PINS) { analogWrite(p, n); } else if (p < DIGITAL_PINS) { digitalWrite(p, n); } - else if (p < DIGITAL_PINS+NO_REGS ) { reg[p-DIGITAL_PINS] = n; } - else if (p >= DIGITAL_PINS+NO_REGS ) { EEPROM[p-(DIGITAL_PINS+NO_REGS)]=n;} + else if (p < NUM_PINS+NO_REGS ) { reg[p-NUM_PINS] = n; } + else if (p >= NUM_PINS+NO_REGS ) { EEPROM[p-(NUM_PINS+NO_REGS)]=n;} break; case '?': //get information if (pc) { //executing @@ -291,9 +318,13 @@ void loop(){ break; case 'I': //set pin n input pinMode(n,INPUT); + if (n>DIGITAL_PINS) n = analogRead(n); + else n = digitalRead(n); break; case 'P': //set pin n input with pullup pinMode(n,INPUT_PULLUP); + if (n>DIGITAL_PINS) n = analogRead(n); + else n = digitalRead(n); break; case 'A': //set pin p to analog output value n pinMode(p,OUTPUT); From 1f2d6146d00e32bd98b2582bd7bb6bde688eeba4 Mon Sep 17 00:00:00 2001 From: JamesNewton Date: Tue, 9 Jul 2024 15:35:56 -0700 Subject: [PATCH 5/5] Double double quotes quotes a quote... character. e.g. a"ab""cd.". makes a point to: ab"cd. --- Arduino_Dynamixel_Controller.ino | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/Arduino_Dynamixel_Controller.ino b/Arduino_Dynamixel_Controller.ino index 04081c9..a32aee2 100644 --- a/Arduino_Dynamixel_Controller.ino +++ b/Arduino_Dynamixel_Controller.ino @@ -50,7 +50,8 @@ long radix; int sign; char cmd; //the current command byte code char op; //the current operation -bool quoting, debugging; +enum Q {quote_off, quote_on, quote_end } quoting; //track in quotes, just ended, or well past. +bool debugging; #define NO_REGS 26 long reg[NO_REGS]; //registers, each is a long, several bytes int pc; //program counter. Indexes EEPROM @@ -218,22 +219,29 @@ void loop(){ while ((DEBUG_SERIAL.available()) //if data has arrived, || (pc > 0)//or we are running from EEPROM ) { - int c = pc?(char)EEPROM.read(pc++):DEBUG_SERIAL.read(); //get the data + int c = pc?(char)EEPROM.read(pc++):DEBUG_SERIAL.read(); //get the next opcode cmd = char(c); if (debugging) DEBUG_SERIAL.print(cmd); if ('\"'==cmd) { //quote - if (!quoting) { //this is a new quote + if (quoting == quote_off) {//this is a new quote //if (NUM_PINS >= n || REG_SIZE+NUM_PINS <= n) {DEBUG_SERIAL.print("reg?");} reg[n-NUM_PINS]=REGLTR('m'); //reg being defined points to mem at reg m - quoting = true; - } else { //end of a quote + quoting = quote_on; + continue; + } + if (quoting == quote_on) { //end of a quote? EEPROM.put(EEPROM.length()-sizeof(reg[0]), REGLTR('m')); //save the number of bytes we've used - quoting = false; + quoting = quote_end; if (debugging) printreg(n-NUM_PINS); + continue; } - continue; //in either case, we are done here - } - if (quoting) { //if we are quoting, just put it away + if (quoting == quote_end) { //quote JUST ended + quoting = quote_on; //this was a double double, restart + } //go on and save the quote character. + } else if (quoting == quote_end) { //not a quote, and we just ended one + quoting = quote_off; //so we are really done now, it couldn't be a double double + } //keep going + if (quoting == quote_on) { //if we are quoting, just put it away EEPROM.update(REGLTR('m'),cmd); //add the current address, then increment it REGLTR('m')++; continue; //and do nothing more @@ -249,7 +257,6 @@ void loop(){ } n=0; cmd=0;//clear command and value (but not p) at end of line. d=0; //delay doesn't last past one line - quoting = false; skipping = false; //only skip to the end of the line. DEBUG_SERIAL.print("cr"); continue; //loop now, no delay