276 lines
8.6 KiB
C++
276 lines
8.6 KiB
C++
#include "DynamixelSerial.h"
|
|
|
|
//TLC5940NT pin definitions
|
|
#define VPRG 2
|
|
#define SIN 11
|
|
#define SCLK 13
|
|
#define XLAT 4
|
|
#define BLANK 5
|
|
#define DCPRG 6
|
|
#define GSCLK 7
|
|
|
|
#define LIMIT 80
|
|
#define RES 10
|
|
|
|
#define pin_dinamyxel 3
|
|
long int VEL = 500;
|
|
int Temperature,Voltage,Position;
|
|
|
|
long level[16] = {
|
|
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
|
|
|
|
float Sensor[4] = { 0, 0, 0, 0};
|
|
|
|
const int remap[16] = {
|
|
3,4,5,6,7,8,9,10,11,12,0,1,2,13,14,15};
|
|
|
|
float MAX[4] = { 0, 0, 0, 0};
|
|
|
|
#define FREQ 14
|
|
#if FREQ > 1
|
|
byte int_counter = 0;
|
|
#endif
|
|
|
|
|
|
char spi_transfer(volatile byte data)
|
|
{
|
|
SPDR = data; // Start the transmission
|
|
while (!(SPSR & (1<<SPIF))) // Wait the end of the transmission
|
|
{
|
|
};
|
|
return SPDR; // return the received byte
|
|
}
|
|
|
|
void makemagic(){
|
|
setGreys();
|
|
feedPorts();
|
|
}
|
|
|
|
void feedPorts() {
|
|
// Clock for TLC5940's PWM
|
|
digitalWrite(BLANK, HIGH);
|
|
digitalWrite(BLANK, LOW); //=all outputs ON, start PWM cycle
|
|
for (int i=0; i<4096; i++) {
|
|
pulseGSCLK();
|
|
}
|
|
}
|
|
|
|
void pulseGSCLK() {
|
|
//ultra fast pulse trick, using digitalWrite caused flickering
|
|
PORTD |= 0x80 ; // bring pin 7 high, but don't touch any of the other pins in PORTB
|
|
//16 nanosecs is the min pulse width for the 5940, but no pause seems needed here
|
|
PORTD &= 0x7F; // bring pin 7 low without touching the other pins in PORTB
|
|
}
|
|
|
|
void setGreys() {
|
|
digitalWrite(BLANK, HIGH);
|
|
digitalWrite(XLAT,LOW);
|
|
for(int i = 7; i>=0; i--){
|
|
spi_transfer( (level[2*i+1] & 0x0FF0) >> 4 );
|
|
spi_transfer( ((level[2*i+1] & 0xF) << 4) | ((level[2*i] & 0x0F00) >> 8) );
|
|
spi_transfer( level[2*i] & 0xFF);
|
|
}
|
|
digitalWrite(XLAT,HIGH);
|
|
digitalWrite(XLAT,LOW);
|
|
digitalWrite(BLANK, LOW);
|
|
}
|
|
|
|
|
|
void startup(){
|
|
for(byte x = 0; x < 7; ++x){
|
|
led_white(x, 4095);
|
|
delay(1000);
|
|
led_white(x, 0);
|
|
}
|
|
}
|
|
|
|
void led_rgb(long int red, long int green, long int blue){
|
|
level[remap[0]] = red;
|
|
level[remap[1]] = green;
|
|
level[remap[2]] = blue;
|
|
}
|
|
|
|
void led_white(byte pin, long int intensity){
|
|
level[remap[pin + 3 - 1]] = intensity;
|
|
}
|
|
|
|
void leds_white_on(){
|
|
led_white(1, 4095);
|
|
led_white(2, 4095);
|
|
led_white(3, 4095);
|
|
led_white(4, 4095);
|
|
led_white(5, 4095);
|
|
led_white(6, 4095);
|
|
}
|
|
|
|
void leds_white_off(){
|
|
led_white(1, 0);
|
|
led_white(2, 0);
|
|
led_white(3, 0);
|
|
led_white(4, 0);
|
|
led_white(5, 0);
|
|
led_white(6, 0);
|
|
}
|
|
|
|
void Dynamixel_stop(){
|
|
Dynamixel.turn(1,RIGTH,0); //0 a 1020
|
|
Dynamixel.turn(2,RIGTH,0);
|
|
Dynamixel.turn(3,RIGTH,0);
|
|
Dynamixel.turn(4,RIGTH,0);
|
|
Dynamixel.turn(5,RIGTH,0);
|
|
Dynamixel.turn(6,RIGTH,0);
|
|
}
|
|
|
|
void Dynamixel_crazy(){
|
|
Dynamixel.turn(1,RIGTH,map(Sensor[0], MAX[0] , LIMIT, 300, 1000)); //0 a 1020
|
|
Dynamixel.turn(2,LEFT, map(Sensor[0] + Sensor[1], MAX[0] + MAX[1] , 2*LIMIT, 300, 1000));
|
|
Dynamixel.turn(3,RIGTH,map(Sensor[1], MAX[1] , LIMIT, 300, 1000));
|
|
Dynamixel.turn(4,LEFT, map(Sensor[2], MAX[2] , LIMIT, 300, 1000));
|
|
Dynamixel.turn(5,RIGTH,map(Sensor[2] + Sensor[3], MAX[2] + MAX[3] , 2*LIMIT, 300, 1000));
|
|
Dynamixel.turn(6,LEFT, map(Sensor[3], MAX[3] , LIMIT, 300, 1000));
|
|
}
|
|
|
|
float measure(int anaPin) {
|
|
if ((anaPin == 1)||(anaPin == 2)) digitalWrite(19, HIGH);
|
|
else digitalWrite(18, HIGH);
|
|
delay(49);
|
|
float average = analogRead(anaPin)/2.;
|
|
if ((anaPin == 1)||(anaPin == 2)) digitalWrite(19,LOW);
|
|
else digitalWrite(18, LOW);
|
|
return(average*2.54);
|
|
}
|
|
|
|
void sensor_update()
|
|
{
|
|
for (int i = 0; i<4; i++) Sensor[i]=measure(i);
|
|
// Serial.begin(9600); // Begin Serial Comunication
|
|
// for (int i = 0; i<4; i++)
|
|
// {
|
|
// Serial.print(Sensor[i]);
|
|
// Serial.print(' ');
|
|
// }
|
|
// Serial.println();
|
|
// delay(100);
|
|
//Dynamixel.begin(1000000,pin_dinamyxel); // Inicialize the servo at 1Mbps and Pin Control 2
|
|
}
|
|
|
|
|
|
//The timer interrupt routine, which periodically interprets the serial commands
|
|
ISR(TIMER2_OVF_vect) {
|
|
sei(); //Reenable global interrupts, otherwise serial commands will get dropped
|
|
#if FREQ > 1
|
|
if(++int_counter == FREQ){ // Only do this once every FREQ-th interrupt
|
|
int_counter = 0;
|
|
#endif //FREQ
|
|
|
|
makemagic();
|
|
makemagic();
|
|
makemagic();
|
|
makemagic();
|
|
|
|
#if FREQ > 1
|
|
}
|
|
#endif //FREQ
|
|
}
|
|
|
|
unsigned long time;
|
|
|
|
void setup(){
|
|
pinMode(VPRG, OUTPUT);
|
|
pinMode(SIN, OUTPUT);
|
|
pinMode(SCLK, OUTPUT);
|
|
pinMode(XLAT, OUTPUT);
|
|
pinMode(BLANK, OUTPUT);
|
|
pinMode(DCPRG, OUTPUT);
|
|
pinMode(GSCLK, OUTPUT);
|
|
pinMode(MISO, INPUT);
|
|
pinMode(SS,OUTPUT);
|
|
pinMode(19,OUTPUT);
|
|
pinMode(18,OUTPUT);
|
|
digitalWrite(SS,HIGH); //disable device
|
|
digitalWrite(SIN, LOW);
|
|
digitalWrite(SCLK, LOW);
|
|
digitalWrite(XLAT, LOW);
|
|
digitalWrite(VPRG, LOW);
|
|
digitalWrite(19, LOW);
|
|
digitalWrite(18, LOW);
|
|
digitalWrite(BLANK, HIGH);
|
|
digitalWrite(GSCLK, HIGH);
|
|
digitalWrite(DCPRG, LOW); // USE EEPROM DC register if LOW
|
|
Dynamixel.begin(1000000,pin_dinamyxel); // Inicialize the servo at 1Mbps and Pin Control 2
|
|
delay(1000);
|
|
for(int i=1; i<7; i++) Dynamixel.setEndless(i, ON);
|
|
|
|
TCCR2A = 0;
|
|
TCCR2B = 1<<CS22 | 0<<CS21 | 0<<CS20;
|
|
|
|
//Timer2 Overflow Interrupt Enable
|
|
TIMSK2 = 1<<TOIE2;
|
|
delay(10);
|
|
|
|
//Setup the Hardware SPI registers
|
|
// SPCR = 01010000
|
|
//interrupt disabled,spi enabled,msb 1st,master,clk low when idle,
|
|
//sample on leading edge of clk,system clock/4 (fastest)
|
|
byte clr;
|
|
SPCR = (1<<SPE)|(1<<MSTR);
|
|
clr=SPSR;
|
|
clr=SPDR;
|
|
delay(10);
|
|
|
|
|
|
led_rgb(0, 4095, 0);
|
|
|
|
for (int i = 0; i<4; i++)
|
|
{
|
|
MAX[i] = measure(i);
|
|
if (MAX[i]>450) MAX[i] = 450;
|
|
}
|
|
|
|
Dynamixel_stop();
|
|
leds_white_off();
|
|
time = millis();
|
|
// Dynamixel.turn(1,LEFT,600);
|
|
// Dynamixel.turn(2,LEFT,600);
|
|
// Dynamixel.turn(3,LEFT,600);
|
|
// Dynamixel.turn(4,LEFT,600);
|
|
// Dynamixel.turn(5,LEFT,600);
|
|
// Dynamixel.turn(6,LEFT,600);
|
|
|
|
}
|
|
|
|
|
|
|
|
void loop(){
|
|
|
|
sensor_update();
|
|
|
|
if ((Sensor[0]<LIMIT)||(Sensor[1]<LIMIT)||(Sensor[2]<LIMIT)||(Sensor[3]<LIMIT)) { Dynamixel_stop(); leds_white_on(); led_rgb(4095, 0, 0);}
|
|
else if ((Sensor[0]<(MAX[0] - RES))&&(Sensor[1]<(MAX[1] - RES))&&(Sensor[2]<(MAX[2] - RES))&&(Sensor[3]<(MAX[3] - RES))) { time = millis(); Dynamixel_crazy(); leds_white_on(); led_rgb(4095, 4095, 0);}
|
|
else if ((Sensor[0]<(MAX[0] - RES))&&(Sensor[1]>=(MAX[1] - RES))&&(Sensor[2]>=(MAX[2] - RES))&&(Sensor[3]>=(MAX[3] - RES))) { time = millis(); Dynamixel.turn(1,LEFT,map(Sensor[0], MAX[0] , LIMIT, 300, 1000)); led_white(1,4095); led_rgb(0, 0, 4095);}
|
|
else if ((Sensor[0]>=(MAX[0] - RES))&&(Sensor[1]<(MAX[1] - RES))&&(Sensor[2]>=(MAX[2] - RES))&&(Sensor[3]>=(MAX[3] - RES))) { time = millis(); Dynamixel.turn(3,LEFT,map(Sensor[1], MAX[1] , LIMIT, 300, 1000)); led_white(3,4095); led_rgb(4095, 0, 4095);}
|
|
else if ((Sensor[0]>=(MAX[0] - RES))&&(Sensor[1]>=(MAX[1] - RES))&&(Sensor[2]<(MAX[2] - RES))&&(Sensor[3]>=(MAX[3] - RES))) { time = millis(); Dynamixel.turn(4,LEFT,map(Sensor[2], MAX[2] , LIMIT, 300, 1000)); led_white(4,4095); led_rgb(0, 4095, 4095);}
|
|
else if ((Sensor[0]>=(MAX[0] - RES))&&(Sensor[1]>=(MAX[1] - RES))&&(Sensor[2]>=(MAX[2] - RES))&&(Sensor[3]<(MAX[3] - RES))) { time = millis(); Dynamixel.turn(6,LEFT,map(Sensor[3], MAX[3] , LIMIT, 300, 1000)); led_white(6,4095); led_rgb(1000, 4095, 4095);}
|
|
else if ((millis()- time)>2000){ leds_white_off(); Dynamixel_stop(); led_rgb(0, 4095, 0);}
|
|
|
|
|
|
|
|
// if (Sensor[0]<LIMIT) { Dynamixel.turn(1,LEFT,0); led_white(1,4095); led_rgb(4095, 0, 0);}
|
|
// else if ((Sensor[0]>=LIMIT)&&(Sensor[0]<(MAX[0] - RES))){Dynamixel.turn(1,LEFT,map(Sensor[0], MAX[0] , LIMIT, 300, 1000)); led_white(1,4095); led_rgb(4095, 4095, 0); }
|
|
// else if (Sensor[0]>=MAX[0]){Dynamixel.turn(1,LEFT,0); led_white(1,0); led_rgb(0, 4095, 0);}
|
|
//
|
|
// if (Sensor[1]<LIMIT) {Dynamixel.turn(3,RIGTH,0); led_white(3,4095); led_rgb(4095, 0, 0);}
|
|
// else if ((Sensor[1]>=LIMIT)&&(Sensor[1]<(MAX[1] - RES))){Dynamixel.turn(3,LEFT,map(Sensor[1], MAX[1] , LIMIT, 300, 1000)); led_white(3,4095); led_rgb(4095, 4095, 4095);}
|
|
// else if (Sensor[1]>=MAX[1]){Dynamixel.turn(3,LEFT,0); led_white(3,0); led_rgb(0, 4095, 0);}
|
|
//
|
|
// if (Sensor[2]<LIMIT) {Dynamixel.turn(4,LEFT,0); led_white(4,4095); led_rgb(4095, 0, 0);}
|
|
// else if ((Sensor[2]>=LIMIT)&&(Sensor[2]<(MAX[2] - RES))){Dynamixel.turn(4,LEFT,map(Sensor[2], MAX[2] , LIMIT, 300, 1000)); led_white(4,4095); led_rgb(0, 4095, 4095);}
|
|
// else if (Sensor[2]>=MAX[2]){Dynamixel.turn(4,LEFT,0); led_white(4,0); led_rgb(0, 4095, 0);}
|
|
//
|
|
// if (Sensor[3]<LIMIT) {Dynamixel.turn(6,RIGTH,0); led_white(6,4095); led_rgb(4095, 0, 0);}
|
|
// else if ((Sensor[3]>=LIMIT)&&(Sensor[3]<(MAX[3] - RES))){Dynamixel.turn(6,LEFT,map(Sensor[3], MAX[3] , LIMIT, 300, 1000)); led_white(6,4095); led_rgb(0, 0, 4095);}
|
|
// else if (Sensor[3]>=MAX[3]) {Dynamixel.turn(6,LEFT,0); led_white(6,0); led_rgb(0, 4095, 0);}
|
|
|
|
|
|
}
|