/* Snakebot joint board code. RCN started 4/22/2017 */ #include #include #define SERVO1_PIN 11 #define SERVO2_PIN 10 #define SERVO3_PIN 9 #define SERVO4_PIN 3 Servo servo1, servo2, servo3, servo4; #define CENTER_POS 1500 #define MAX_POS 1700 #define MIN_POS 1300 // Servo command positions in PWM microseconds. 1500 = center/straight. // On sample gearbox A, 1 degree = about 10.36 usec, 10 usec = about .964 degrees, +-1%. int servo1_set = CENTER_POS; int servo2_set = CENTER_POS; int servo3_set = CENTER_POS; int servo4_set = CENTER_POS; #define INT_LED 13 #define RED_LED 5 #define YELLOW_LED 6 #define GREEN_LED 7 #define BLUE_LED 8 // Turns out software serial is unreliable with this program. // Individual bits seem to fail randomly on one in 10-20 transfers. // Possibly due to interference between interrupts and all the timing commands. //#define rx_fwd 14 // (MISO) //#define tx_fwd 15 // (SCK) //SoftwareSerial fwd_serial(rx_fwd, tx_fwd); #define SYNCH_PIN A0 // Broadcast synch signal from brain if we need it #define COMMAND_PIN1 A1 // fwd/rev #define COMMAND_PIN2 A2 #define COMMAND_PIN3 A3 // Commands // These represent the raw bits of the command lines, and cannot be arbitrarily changed. #define CMD_STOP_FWD 0 // 000 #define CMD_STOP_REV 1 // 001 #define CMD_ADJUST_FWD 2 // 010 #define CMD_ADJUST_REV 3 // 011 #define CMD_MOVE_FWD 4 // 100 #define CMD_MOVE_REV 5 // 101 #define CMD_NO_INPUT 7 // 111 All channels pullup when no signal is present int command_bit1, command_bit2, command_bit3; int cur_command = CMD_MOVE_FWD; // move initialization for testing int prev_command = CMD_MOVE_FWD; int command_stable = 0; // How many cycles has current command been stable // States #define ST_STOP_FWD 10 #define ST_STOP_REV 20 #define ST_ADJUST_FWD 30 #define ST_ADJUST_REV 40 #define ST_MOVE_FWD 50 #define ST_MOVE_REV 60 #define ST_FWD_TO_REV 70 #define ST_REV_TO_FWD 80 int current_state = ST_STOP_FWD; int current_state_count = 0; #define MAX_ADJUST 3 bool adjust_complete = false; // Heartbeat timing #define HEARTBEAT_USEC 10000ul // 10 milliseconds = 100Hz unsigned long loop_start_usec; unsigned long loop_restart_usec; unsigned long work_done_usec; unsigned long usec_used; unsigned long delay_usec; unsigned long delay_ms; unsigned int remainder_usec; unsigned int count100 = 0; unsigned int count2 = 0; #define POS_MEM_SIZE 160 int wave_array[POS_MEM_SIZE]; int servo1_index = 40; int servo2_index = 30; int servo3_index = 20; int servo4_index = 10; int out_index = 0; int signal_in = LOW; bool high_signal_seen = false; bool signal_received = false; //---------------------------------------------------------------------------------------------------- // the setup function runs once when you press reset or power the board void setup() { int i,j,k; long ilong; pinMode(INT_LED, OUTPUT); pinMode(RED_LED, OUTPUT); pinMode(YELLOW_LED, OUTPUT); pinMode(GREEN_LED, OUTPUT); pinMode(BLUE_LED, OUTPUT); pinMode(SYNCH_PIN, INPUT); pinMode(COMMAND_PIN1, INPUT); pinMode(COMMAND_PIN2, INPUT); pinMode(COMMAND_PIN3, INPUT); // Initialize drive tables for(i = 0; i < POS_MEM_SIZE; i++) { wave_array[i] = CENTER_POS; // safe initial values } } //---------------------------------------------------------------------------------------------------------------------- // the loop function runs over and over again forever void loop() { // initialization/startup stuff // Do some light-blinking stuff before activating digitalWrite(RED_LED, HIGH); delay(1000); digitalWrite(YELLOW_LED, HIGH); delay(1000); digitalWrite(GREEN_LED, HIGH); delay(1000); digitalWrite(BLUE_LED, HIGH); delay(1000); digitalWrite(RED_LED, LOW); digitalWrite(YELLOW_LED, LOW); digitalWrite(GREEN_LED, LOW); digitalWrite(BLUE_LED, LOW); delay(1000); // Attach servos servo1.attach(SERVO1_PIN, 900, 2100); // Range of HiTec HS-5070MH servo ?? delay(50); servo2.attach(SERVO2_PIN, 900, 2100); // Range of HiTec HS-5070MH servo ?? delay(50); servo3.attach(SERVO3_PIN, 900, 2100); // Range of HiTec HS-5070MH servo ?? delay(50); servo4.attach(SERVO4_PIN, 900, 2100); // Range of HiTec HS-5070MH servo ?? delay(50); servo1_set = CENTER_POS; servo2_set = CENTER_POS; servo3_set = CENTER_POS; servo4_set = CENTER_POS; update_servos(); // Set to safe initial value // Set up the built-in serial port // For micro, calls must be to Serial1 to acces Rx and Tx pins Serial1.begin(115200); while(!Serial1); // Set up SoftwareSerial port for forward movement // Which turns out to be unreliable, possibly due to dense timing calls //pinMode(rx_fwd, INPUT_PULLUP); //pinMode(tx_fwd, OUTPUT); //fwd_serial.begin(57600); // Max supported baud rate //fwd_serial.listen(); // Empty buffer and start listening for input on the fwd_serial port //while(!fwd_serial.isListening()); // Loop until port activates count100 = 0; count2 = 0; while(true) // Start local infinite loop running at about 100 Hz { loop_start_usec = micros(); // Flash LED to indicate various state information. // Start by turning everything off. // Some will be turned back on almost immediately with probably imperceptible flicker. digitalWrite(RED_LED, LOW); digitalWrite(YELLOW_LED, LOW); digitalWrite(GREEN_LED, LOW); digitalWrite(BLUE_LED, LOW); // Heartbeat: Flash red once per second, on 500ms, off 500ms. if(count100 < 50) digitalWrite(RED_LED, HIGH); // If all the wave-array values are center, indicating straight-snake position // in the serial command line, flash green in unison with heartbeat. // This means adjust should move towards straight. // Also it should be OK to initiate slither if snake is straight. if(wave_array[servo1_index] == CENTER_POS && wave_array[servo2_index] == CENTER_POS && wave_array[servo3_index] == CENTER_POS && wave_array[servo4_index] == CENTER_POS) { if(count100 < 50) digitalWrite(GREEN_LED, HIGH); } // Flash brief yellow to indicate adjust mode, green for forward. if(current_state == ST_ADJUST_FWD) { if(count100 < 10) digitalWrite(YELLOW_LED, HIGH); } if(current_state == ST_MOVE_FWD) { if(count100 < 10) digitalWrite(GREEN_LED, HIGH); } // Branch according to current state if(current_state == ST_STOP_FWD) do_stop_fwd(); else if(current_state == ST_ADJUST_FWD) do_adjust_fwd(); else if(current_state == ST_MOVE_FWD) do_move_fwd(); // Wait until time to start next heartbeat cycle work_done_usec = micros(); if(work_done_usec > loop_start_usec) usec_used = work_done_usec - loop_start_usec; else usec_used = (0xFFFFFFFFul - loop_start_usec) + work_done_usec; // usec counter loops after ~70 min if(usec_used >= HEARTBEAT_USEC) delay_usec = 0; else delay_usec = (HEARTBEAT_USEC - usec_used); if(delay_usec < 15000) delayMicroseconds((unsigned int)delay_usec); else { delay_ms = delay_usec/1000; remainder_usec = (unsigned int)(delay_usec - (delay_ms * 1000)); delay(delay_ms); // because delayMicroseconds does not work for values > 16383 delayMicroseconds(remainder_usec); } count100++; if(count100 >= 100) count100 = 0; count2++; if(count2 >= 2) count2 = 0; } // End local infinite loop } // End loop() function //---------------------------------------------------------------------------------------------- //********************************************************************************************** //---------------------------------------------------------------------------------------------- void read_command() // Reads the command lines and determines the current command being issued by the brain // Since it is possible for a multi-bit change to be split over two cycles, command stability is tracked. // Modifies global variables cur_command, prev_command, and command_stable. { int in_val; in_val = digitalRead(COMMAND_PIN1); if(in_val == HIGH) command_bit1 = 1; else command_bit1 = 0; in_val = digitalRead(COMMAND_PIN2); if(in_val == HIGH) command_bit2 = 1; else command_bit2 = 0; in_val = digitalRead(COMMAND_PIN3); if(in_val == HIGH) command_bit3 = 1; else command_bit3 = 0; prev_command = cur_command; cur_command = (command_bit3<<2) | (command_bit2<<1) | command_bit1; if(cur_command == prev_command) command_stable++; //cur_command = CMD_MOVE_FWD; } void process_serial() // Check the serial input to see if a new position has arrived. // If so read it, write the outgoing value to the serial output, // and update the servo goal positions. // Note that it takes 2 bytes to transmit a position (900 - 2100). // They are transmitted LSB (least significant byte) first. // Modifies the global variables servo*_goal. { byte serial_in_lsb, serial_in_msb, serial_out_lsb, serial_out_msb; int new_pos, out_pos; if(Serial1.available() >= 2) // if(fwd_serial.available() >= 2) { // Read 2 bytes and convert to incoming position serial_in_lsb = Serial1.read(); serial_in_msb = Serial1.read(); //serial_in_lsb = fwd_serial.read(); // Software serial is glitchy //serial_in_msb = fwd_serial.read(); new_pos = (int)(((unsigned int)serial_in_lsb) | (((unsigned int)serial_in_msb) << 8)); digitalWrite(BLUE_LED, HIGH); // flash blue if value has been received // Write outgoing servo position to serial out_pos = wave_array[out_index]; serial_out_lsb = (byte)(out_pos & 0xFF); serial_out_msb = (byte)((out_pos >> 8) & 0xFF); Serial1.write(serial_out_lsb); Serial1.write(serial_out_msb); //fwd_serial.write(serial_out_lsb); // Software serial is glitchy //fwd_serial.write(serial_out_msb); // Update wave array servo1_index++; if(servo1_index >= POS_MEM_SIZE) servo1_index = 0; wave_array[servo1_index] = new_pos; servo2_index++; if(servo2_index >= POS_MEM_SIZE) servo2_index = 0; servo3_index++; if(servo3_index >= POS_MEM_SIZE) servo3_index = 0; servo4_index++; if(servo4_index >= POS_MEM_SIZE) servo4_index = 0; out_index++; if(out_index >= POS_MEM_SIZE) out_index = 0; } } //---------------------------------------------------------------------------------------------- void servo_move() // Move servos directly to goal position. Modifies global variables servo*_set { servo1_set = wave_array[servo1_index]; servo2_set = wave_array[servo2_index]; servo3_set = wave_array[servo3_index]; servo4_set = wave_array[servo4_index]; update_servos(); } void servo_adjust() // Adjust servos towards goal position by bounded amount. // Used to produce a large change without sudden jerks. // Modifies global variables servo*_set and adjust_complete. { int servo1_goal, servo2_goal, servo3_goal, servo4_goal; adjust_complete = true; servo1_goal = wave_array[servo1_index]; servo2_goal = wave_array[servo2_index]; servo3_goal = wave_array[servo3_index]; servo4_goal = wave_array[servo4_index]; if(abs(servo1_set - servo1_goal) <= MAX_ADJUST) servo1_set = servo1_goal; else { adjust_complete = false; if(servo1_set < servo1_goal) servo1_set += MAX_ADJUST; else servo1_set -= MAX_ADJUST; } if(abs(servo2_set - servo2_goal) <= MAX_ADJUST) servo2_set = servo2_goal; else { adjust_complete = false; if(servo2_set < servo2_goal) servo2_set += MAX_ADJUST; else servo2_set -= MAX_ADJUST; } if(abs(servo3_set - servo3_goal) <= MAX_ADJUST) servo3_set = servo3_goal; else { adjust_complete = false; if(servo3_set < servo3_goal) servo3_set += MAX_ADJUST; else servo3_set -= MAX_ADJUST; } if(abs(servo4_set - servo4_goal) <= MAX_ADJUST) servo4_set = servo4_goal; else { adjust_complete = false; if(servo4_set < servo4_goal) servo4_set += MAX_ADJUST; else servo4_set -= MAX_ADJUST; } update_servos(); } void update_servos() // Instructs all servos to set values. References global variables servo*_set. { int servo1_com, servo2_com, servo3_com, servo4_com; servo1_com = servo1_set; if(servo1_com > MAX_POS) servo1_com = MAX_POS; if(servo1_com < MIN_POS) servo1_com = MIN_POS; //servo2_com = servo2_set; servo2_com = 1500 + (1500 - servo2_set); // servos 2 and 4 are inverted. if(servo2_com > MAX_POS) servo2_com = MAX_POS; if(servo2_com < MIN_POS) servo2_com = MIN_POS; servo3_com = servo3_set; if(servo3_com > MAX_POS) servo3_com = MAX_POS; if(servo3_com < MIN_POS) servo3_com = MIN_POS; //servo4_com = servo4_set; servo4_com = 1500 + (1500 - servo4_set); if(servo4_com > MAX_POS) servo4_com = MAX_POS; if(servo4_com < MIN_POS) servo4_com = MIN_POS; servo1.writeMicroseconds(servo1_com); servo2.writeMicroseconds(servo2_com); servo3.writeMicroseconds(servo3_com); servo4.writeMicroseconds(servo4_com); } //-------------------------------------------------------------------------------------------------------------------- // Routines implementing differential details of various states void do_stop_fwd() // In the stop_fwd state, servo position goals can be passed down the servo chain, // but no movement is executed. This allows a new global pose to be loaded by the brain. // State can transition to adjust_fwd, but not move_fwd, so as to avoid sudden jerks. // Accesses global variables cur_command, command_stable, current_state, and current_state_count { if(current_state != ST_STOP_FWD) return; // If somehow we got here by mistake... current_state_count++; process_serial(); // Take care of any waiting serial input read_command(); // Read broadcast command if(cur_command == CMD_STOP_FWD) return; // if no command change, we are done if(command_stable < 2) return; // If the command is not stable, stay in current state // Otherwise, branch based on new command if(cur_command == CMD_ADJUST_FWD || cur_command == CMD_MOVE_FWD) { current_state = ST_ADJUST_FWD; current_state_count = 0; } /* else if(cur_command == CMD_STOP_REV || cur_command == CMD_ADJUST_REV || cur_command == CMD_MOVE_REV) { // current_state = FWD_TO_REV; // Eventually. For now, we just hold in stop_fwd. // current_state_count = 0; } else if(cur_command == CMD_NO_INPUT) { // Stay in stop_fwd } else { // Unrecognized command. Just stay in stop_fwd // Or maybe do some error signal // Don't increment current state count for now... } */ } void do_stop_rev() { return; } void do_adjust_fwd() // In the adjust_fwd state, servo position goals may be passed down the servo chain, // allowing new pose information to be loaded by the brain. // In addition, the joints are moved towards their goals, a maximum of MAX_ADJUST usec per 10 ms. // Idea is to effect some goal pose without sudden jerks. // Can transition to STOP_FWD at any point, and to MOVE_FWD but only if adjustment is more or less complete. // Accesses global variables cur_command, command_stable, current_state, and current_state_count. // also count2 and adjust_complete. { if(current_state != ST_ADJUST_FWD) return; // If somehow we got here by mistake... current_state_count++; process_serial(); // Take care of any waiting serial input if(count2 == 0) // Update servos every two heartbeats (20ms) which is maximum rate that makes sense { servo_adjust(); } read_command(); // Read broadcast command if(cur_command == CMD_ADJUST_FWD) return; // if no command change, we are done if(command_stable < 2) return; // If the command is not stable, stay in current state // Otherwise, branch based on new command if(cur_command == CMD_STOP_FWD) { current_state = ST_STOP_FWD; current_state_count = 0; } else if(cur_command == CMD_MOVE_FWD) { //if(adjust_complete == true) { current_state = ST_MOVE_FWD; current_state_count = 0; } } /* else if(cur_command == CMD_STOP_REV || cur_command == CMD_ADJUST_REV || cur_command == CMD_MOVE_REV) { current_state = ST_STOP_FWD; // Reverse transitions are accessed via stop_fwd current_state_count = 0; } else if(cur_command == CMD_NO_INPUT) { // Transition to stop_fwd if no command (brain goes offline) current_state = ST_STOP_FWD; current_state_count = 0; } else { // Unrecognized command. Transition to stop_fwd. // Separate case to allow some error signal if we want to current_state = ST_STOP_FWD; current_state_count = 0; } */ } void do_move_fwd() // In the move_fwd state, servo position goals are passed down the servo chain. // The servo positions are constantly updated (every 20 ms) to whatever the current goal position is. // This allows a movement wave to be streamed by the brain. // Can transition to STOP_FWD or ADJUST_FWD at any point. // Accesses global variables cur_command, command_stable, current_state, and current_state_count. // also count2. { if(current_state != ST_MOVE_FWD) return; // If somehow we got here by mistake... current_state_count++; process_serial(); // Take care of any waiting serial input if(count2 == 0) // Update servos every two heartbeats (20ms) which is maximum rate that makes sense { servo_move(); } read_command(); // Read broadcast command if(cur_command == CMD_MOVE_FWD) return; // if no command change, we are done if(command_stable < 2) return; // If the command is not stable, stay in current state // Otherwise, branch based on new command if(cur_command == CMD_STOP_FWD) { current_state = ST_STOP_FWD; current_state_count = 0; } else if(cur_command == CMD_ADJUST_FWD) { current_state = ST_ADJUST_FWD; current_state_count = 0; } /* else if(cur_command == CMD_STOP_REV || cur_command == CMD_ADJUST_REV || cur_command == CMD_MOVE_REV) { current_state = ST_STOP_FWD; // Reverse transitions are accessed via stop_fwd current_state_count = 0; } else if(cur_command == CMD_NO_INPUT) { // Transition to stop_fwd if no command (brain goes offline) current_state = ST_STOP_FWD; current_state_count = 0; } else { // Unrecognized command. Transition to stop_fwd. // Separate case to allow some error signal if we want to current_state = ST_STOP_FWD; current_state_count = 0; } */ }