I got my Hexy for Christmas. I put it together today. I have loaded. the new firmware. I have connected to it to pomoco (crapper name to type by the way). I haven’t found instructions for the Ping( the ultra sonic sensor.) Also is there a “wander around looking at stuff and avoiding things” Algorithm out there some where. Something simple and subsumtive?
For ultrasonic the forum tell :
Replace
#include "WProgram.h" by #include "Arduino.h" in ultrasonic.h and cpp
Complete firmware with changes (D command return Distance in CM) :
[code]#include “TimerOne.h”
#include “SPI.h”
//////////////////////////////
#include “Ultrasonic.h”
#define TRIGGER_PIN 17
#define ECHO_PIN 11
Ultrasonic ultrasonic(TRIGGER_PIN, ECHO_PIN);
/////////////////////
#define STATUS_LED 7
#define SERVOS 32
#define MAX_TIMINGS 36
#define ON10 PORTB |= (1<<6) // turn on pin 10
#define OFF10 PORTB &= ~(1<<6) // turn off pin 10
#define GROUPS 4
#define SERVOS_PER_GROUP 8
uint16_t group_offsets[4] = {0,251,502,753};
uint8_t group_latches[4] = {5,6,7,4};
uint8_t pin_2_num[8] = {0x08,0x04,0x02,0x01, 0x80,0x40,0x20,0x10};
uint8_t servo_positions[SERVOS];
uint16_t servo_timings[MAX_TIMINGS];
uint8_t shift_output[MAX_TIMINGS];
uint8_t shift_latch[MAX_TIMINGS];
uint16_t timer;
uint8_t counter = 0;
uint8_t pwm_active = 1;
uint8_t update_reg_flag = 0;
///////////////////////
int printDistanceCM()
{
float duration, cm;
digitalWrite(TRIGGER_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN, LOW);
duration = pulseIn(ECHO_PIN, HIGH,3000000);
cm = duration / 29.0 / 2.0;
//Serial.print("CM: ");
//Serial.println(cm);
return cm;
}
/////////////////////////////////
void setup() {
//setup pin modes
DDRF |= 0xF0; // sets pins F7 to F4 as outputs
DDRB = 0xFF; // sets pins B0 to B7 as outputs
pinMode(STATUS_LED,OUTPUT);
//setup PC serial port
Serial.begin(9600);
Serial1.begin(9600);
//delay(100); // delay to establish the usb connection. using the arduino while-serial command doesn’t
// let it boot properly when there’s no USB conenction.
// setup SPI port
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV2);
//start the servo-timing timer and associated interrupt
Timer1.initialize(10); // initialize timer1, and set a period of 10 uS
Timer1.attachInterrupt(callback); // attaches callback() as a timer overflow interrupt
for(uint8_t i=0; i<MAX_TIMINGS; i++){
servo_timings[i] = 0;
shift_output[i] = 0xFF;
shift_latch[i] = 0xFF;
}
for(uint8_t i=0; i<SERVOS; i++){
servo_positions[i] = 0;
}
update_registers();
}
long unsigned int us_counter = 0;
long unsigned int startTime = 0;
long unsigned int currentTime = 0;
long unsigned int last_update = 0;
long unsigned int micros_new(){
return us_counter;
}
long unsigned int millis_new(){
return us_counter/1000;
}
void delay_new(long unsigned int delay_time){
startTime = millis_new();
currentTime = millis_new() - startTime;
while(currentTime < delay_time){
delayMicroseconds(10);
currentTime = millis_new() - startTime;
}
}
void callback(){
cli();
if(timer == servo_timings[counter]){
SPDR = shift_output[counter]; // push the byte to be loaded to the SPI register
//asm(“nop\n\tnop\n\tnop\n\t”); // pause to wait for the spi register to complete its shift out
while(!(SPSR & (1<<SPIF))); //wait till the register completes
PORTF &= ~(shift_latch[counter]); // clock the shift register latch pin low, setting the register
PORTF |= shift_latch[counter]; // clock the shift register latch pin high, ready to be set low next time
counter++;
}
timer++;
us_counter += 10;
if(timer == 1010){
update_reg_flag=1;
}
if(timer == 2000){
update_reg_flag=0;
timer=0;
counter=0;
}
sei();
}
void update_registers(){
while( update_reg_flag != 1){
//asm(“nop\n\tnop\n\tnop\n\tnop\n\tnop\n\tnop\n\tnop\n\tnop\n\tnop\n\tnop\n\tnop\n\tnop\n\t”);
delayMicroseconds(10);
}
for(uint8_t i=0; i<MAX_TIMINGS; i++){ // clear existing registers, so they can be cleanly written
servo_timings[i] = 0;
shift_output[i] = 0xFF;
shift_latch[i] = 0xFF;
}
uint8_t current_timing=0;
uint8_t group=0;
uint8_t group_active_flag=0;
uint8_t servo_num=0;
uint8_t group_on_time = 0;
uint16_t servo_time=0;
uint8_t group_active_servo_count = 0;
// insert active servos into timing array
for(uint8_t group=0; group<GROUPS; group++){ // go through all groups
group_active_flag = 0;
// ---- arrange the servos in the group from lowest to highest timing length -----
// see how many active servos in the group there are
// make a temp copy of postions to organize with
group_active_servo_count = 0;
uint16_t servo_positions_copy[SERVOS_PER_GROUP] = {};
for(uint8_t servo=0; servo<SERVOS_PER_GROUP; servo++){
if(servo_positions[SERVOS_PER_GROUP*group+servo] != 0){
group_active_servo_count++;
}
servo_num = servo+SERVOS_PER_GROUP*group;
servo_positions_copy[servo] = servo_positions[servo_num];
}
// sort them into a new order
uint8_t group_timing_order[SERVOS_PER_GROUP] = {0xFF,0xFF,0xFF,0xFF, 0xFF,0xFF,0xFF,0xFF};
uint8_t low_count=0;
for(uint8_t i=0; i<group_active_servo_count; i++){
uint8_t lowest_position=251;
for(uint8_t j=0; j<SERVOS_PER_GROUP; j++){
if(servo_positions_copy[j] != 0){
if(servo_positions_copy[j] < lowest_position){
lowest_position = servo_positions_copy[j];
group_timing_order[low_count] = j;
}
}
}
servo_positions_copy[group_timing_order[low_count]]=0;
low_count++;
}
// ---- go through the active servos and insert timings---
// turn on all needed servos in the group at the beginning of their pulse
if(group_active_servo_count > 0){
group_on_time = current_timing;
servo_timings[group_on_time] = group_offsets[group];
shift_output[group_on_time] = 0x00;
shift_latch[group_on_time] = (1<<group_latches[group]);
// turn on all the servos that will be turned on
for(uint8_t i=0; i<group_active_servo_count; i++){
shift_output[group_on_time] |= pin_2_num[group_timing_order[i]];
}
current_timing++;
}
for(uint8_t i=0; i<group_active_servo_count; i++){ // go through all active servos in the group
uint8_t servo = group_timing_order[i];
servo_num = servo+SERVOS_PER_GROUP*group;
servo_time = group_offsets[group] + servo_positions[servo_num]; // calculate the servo's time off
shift_output[group_on_time] |= pin_2_num[servo]; // turn on the servo at the group's turn-on time
// go over existing timings for this group, make sure there's not already one with this timing there
uint8_t original = 1;
for(uint8_t j=1; j<group_active_servo_count; j++){
if( servo_timings[j+group_on_time] == servo_time){
shift_output[j+group_on_time] &= (~(pin_2_num[servo])); // turn off the servo at this timing
original=0;
}
}
// create a timing if there's no existing timing
if(original == 1){
servo_timings[current_timing] = servo_time;
shift_output[current_timing] = shift_output[current_timing-1] & (~(pin_2_num[servo])); // turn off the servo at this timing
shift_latch[current_timing] = (1<<group_latches[group]);
current_timing++;
}
}
update_reg_flag=2;
}
// show the final register values
/*
for(uint8_t i=0; i<MAX_TIMINGS; i++){ // clear existing registers, so they can be cleanly written
Serial.print(i);
Serial.print(":\t");
Serial.print(servo_timings[i]);
Serial.print(",\t");
Serial.print(shift_output[i],HEX);
Serial.print(",\t");
Serial.println(shift_latch[i],HEX);
}
*/
}
boolean debug = false;
boolean testMode = false;
boolean servoCounting = false;
boolean posCounting = false;
byte numString[6];
int powers[] = {1,10,100,1000};
byte numCount = 0;
unsigned short total = 0;
short inServo = -1;
short inPos = -1;
void loop() {
digitalWrite(STATUS_LED, HIGH);
delay_new(500);
digitalWrite(STATUS_LED, LOW);
delay_new(500);
for(int i=0; i<32; i++){
changeServo(i,0);
}
changeServo(0,1500);
changeServo(1,1500);
changeServo(2,1500);
changeServo(3,1500);
delay_new(100);
while(true){
TIMSK0 &= ~(_BV(TOIE0)); // disables the arduino delay function, but also
// all but eliminates servo jitter
TIMSK2 &= ~(_BV(TOIE2)); // disable the arduino tone function, but also
// also helps eliminate some jitter
TIMSK3 &= ~(_BV(TOIE3)); // for good measure
TIMSK4 &= ~(_BV(TOIE4)); // for good measure
if(update_reg_flag == 1){
update_registers();
}
else{
if(Serial.available()) {
char inChar = (char)Serial.read();
switch(inChar){
case ‘#’:
ON10;
servoCounting = true;
numCount = 0;
inServo = -1;
inPos = -1;
//Serial.println("# received ");
break;
case ‘P’:
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
posCounting = true;
numCount = 0;
//Serial.println("P received " + inServo);
break;
case ‘\r’:
case ‘\n’:
OFF10;
if(posCounting){
inPos = tallyCount();
posCounting = false;
}
if((inServo >=0)&&(inServo <=31)&&(((inPos >= 500)&&(inPos <= 2500))||(inPos == -1))){
changeServo(inServo,inPos);
if((inServo == 25)||(inServo == 26)){
Serial.println(inServo); // only for debugging
}
inServo = -1;
inPos = -1;
}
numCount = 0;
break;
case 'V':
Serial.println("SERVOTOR32_v1.7");
break;
case 'C':
for(int i=0; i<32; i++){
Serial.println(i);
}
Serial.println("All Centered");
break;
case 'K':
for(int i=0; i<32; i++){
changeServo(i,0);
}
Serial.println("All Turned Off");
break;
case 'L':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
changeServo(inServo, -1);
//Serial.println("L received");
break;
case 'D': //ultrasonic
//printDistanceCM(); //ultrasonic
Serial.println(printDistanceCM());
break;
default:
if((inChar > 47)&&(inChar < 58)){
if(numCount<4){
numString[numCount] = inChar-48;
numCount++;
}
}
break;
}
}
if(Serial1.available()){
char inChar = (char)Serial1.read();
switch(inChar){
case '#':
servoCounting = true;
numCount = 0;
inServo = -1;
inPos = -1;
break;
case 'P':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
posCounting = true;
numCount = 0;
break;
case '\r':
case '\n':
if(posCounting){
inPos = tallyCount();
posCounting = false;
}
if((inServo >=0)&&(inServo <=31)&&(((inPos >= 500)&&(inPos <= 2500))||(inPos == -1))){
changeServo(inServo,inPos);
inServo = -1;
inPos = -1;
}
numCount = 0;
break;
case 'V':
Serial1.println("SERVOTOR32_v1.7");
break;
case 'C':
for(int i=0; i<32; i++){
changeServo(i,1500);
}
Serial1.println("All Centered");
break;
case 'K':
for(int i=0; i<32; i++){
changeServo(i,0);
}
Serial1.println("All Turned Off");
break;
case 'L':
if(servoCounting){
inServo = tallyCount();
servoCounting = false;
}
changeServo(inServo, -1);
break;
case 'D': //ultrasonic
//printDistanceCM(); //ultrasonic
Serial.println(printDistanceCM());
break;
default:
if((inChar > 47)&&(inChar < 58)){
if(numCount<4){
numString[numCount] = inChar-48;
numCount++;
}
}
break;
}
}
}
}
}
void changeServo(byte servo, short pos){
servo_positions[servo] = pos/10;
}
short tallyCount(){
total=0;
for(int i=0; i<numCount; i++){
total += powers[i]*numString[(numCount-1)-i];
}
if(numCount == 0){
total = -1;
}
return total;
}
[/code]
Don’t work for me at that time.
I think the pin 17 & 11 are not true.
Total robotics/arduino newbie here with a really broad question on the ping sensor:
I have no idea how the ping sensor works or what I’ll need to do to get it working ._.
Do I upload the ping code to Hex like I did with the firmware? How do I test the sensor to make sure it’s working properly? Does the sensor output print somewhere?
EDIT 1 - progress so far
I added the ultrasonic library by following the instructions above. I’ll type them out here in the order I did them.
- Downloaded the ultrasonic library and placed it in the arduino library folder. Library is here: http://jaktek.com/wp-content/uploads/2011/12/Ultrasonic.zip
- Replaced the #include “WProgram.h” line in both the ultrasonic.h and the ultrasonic.cpp files of the library with #include “Arduino.h”
- Imported the library to the Servotor sketch. Follow this guide for more.
- Uploaded the modified Servotor sketch
- Uploaded the test code sketch to Hexy.
- Tested the test code sketch in Serial Monitor - it produces an output.
- Finally resolved the sensor issue by plugging it in right
