miguel/Lab_interaccio
1
0
Fork 0
Code Issues Pull requests Actions Packages Projects Releases Wiki Activity
Lab_interaccio/2013/SpamTower_v4/SpamTower_v4.ino
Miguel c6edeac387 second commit
2025-02-25 21:29:42 +01:00

663 lines
18 KiB
C++
Raw Blame History

#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
uint8_t dataPin = 8;
uint8_t latchPin = 9;
uint8_t clockPin = 10;
uint8_t val_shift; //Valor del shift register
#define LIMIT 50
#define RES 30
#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[6] = { 0, 0, 0, 0, 0, 0};
const int remap[16] = {
3,4,5,6,7,8,9,10,11,12,0,1,2,13,14,15};
const int din[7] = {
0,6,5,4,3,2,1};
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 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){
if (pin < 7) level[remap[pin + 3 - 1]] = intensity;
}
void spam_on(){
shiftWrite(1, HIGH);
}
void spam_off(){
shiftWrite(1, LOW);
}
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(din[1],RIGTH,0); //0 a 1020
Dynamixel.turn(din[2],RIGTH,0);
Dynamixel.turn(din[3],RIGTH,0);
Dynamixel.turn(din[4],RIGTH,0);
Dynamixel.turn(din[5],RIGTH,0);
Dynamixel.turn(din[6],RIGTH,0);
}
void Dynamixel_crazy(){
Dynamixel.turn(din[1],RIGTH,map(Sensor[0], MAX[0] , LIMIT, 300, 1000)); //0 a 1020
Dynamixel.turn(din[2],LEFT, map(Sensor[0] + Sensor[1], MAX[0] + MAX[1] , 2*LIMIT, 300, 1000));
Dynamixel.turn(din[3],RIGTH,map(Sensor[1], MAX[1] , LIMIT, 300, 1000));
Dynamixel.turn(din[4],LEFT, map(Sensor[2], MAX[2] , LIMIT, 300, 1000));
Dynamixel.turn(din[5],RIGTH,map(Sensor[2] + Sensor[3], MAX[2] + MAX[3] , 2*LIMIT, 300, 1000));
Dynamixel.turn(din[6],LEFT, map(Sensor[3], MAX[3] , LIMIT, 300, 1000));
}
float measure(int anaPin) {
shiftWrite(6 - anaPin, HIGH);
delay(10);
float average = analogRead(anaPin)/2.;
shiftWrite(6 - anaPin, LOW);
delay(10);
return(average*2.54);
}
void sensor_update()
{
for (int i = 0; i<4; i++) { Sensor[i]=measure(i); /*if ((Sensor[i] > MAX[i])&&(Sensor[i] < 450)) MAX[i] = Sensor[i];*/}
Serial.begin(9600); // Begin Serial Comunication
for (int i = 0; i<4; i++)
{
Serial.print("Sensor");
Serial.print(i);
Serial.print(": ");
// Serial.print(MAX[i]);
// Serial.print(' ');
Serial.print(Sensor[i]);
Serial.print(", ");
}
delay(100);
Serial.println();
delay(100);
Dynamixel.begin(1000000,pin_dinamyxel); // Inicialize the servo at 1Mbps and Pin Control 2
}
void resetshift()
{
val_shift = 0x00;
digitalWrite(latchPin, LOW);
shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
digitalWrite(latchPin, HIGH);
}
void shiftWrite(uint8_t pin, boolean state)
{
bitWrite(val_shift, pin, state);
digitalWrite(latchPin, LOW);
shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
digitalWrite(latchPin, HIGH);
}
//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 timeStop;
unsigned long time_spam;
unsigned long time[15] = { false, false, false, false, false, false, false, false, false, false, false, false, false, false, false};
boolean isTimeOut[4] = { false, false, false, false};
unsigned int timeOut = 0;
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(dataPin, OUTPUT);
pinMode(latchPin, OUTPUT);
pinMode(clockPin, OUTPUT);
pinMode(18, INPUT);
digitalWrite(SS,HIGH); //disable device
digitalWrite(SIN, LOW);
digitalWrite(SCLK, LOW);
digitalWrite(XLAT, LOW);
digitalWrite(VPRG, LOW);
digitalWrite(BLANK, HIGH);
digitalWrite(GSCLK, HIGH);
digitalWrite(DCPRG, LOW); // USE EEPROM DC register if LOW
digitalWrite(18, HIGH);
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();
resetshift();
timeStop = millis();
time_spam = millis();
for (int i = 0; i<15; i++) { time[i] = millis(); }
delay(2000);
spam_off();
// delay(1000);
// spam_on();
}
boolean modeStop = false;
boolean s0 = false;
boolean s1 = false;
boolean s2 = false;
boolean s3 = false;
boolean mode[15] = { false, false, false, false, false, false, false, false, false, false, false, false, false, false, false};
void loop(){
if (!digitalRead(18)) spam_on();
sensor_update();
if ((Sensor[0]<LIMIT)||(Sensor[1]<LIMIT)||(Sensor[2]<LIMIT)||(Sensor[3]<LIMIT)) { Dynamixel_stop(); leds_white_on(); led_rgb(4095, 4095, 4095); modeStop = true; timeStop = millis(); mode[0] = false; mode[1] = false; mode[2] = false; mode[3] = false; mode[4] = false; mode[5] = false;}
else
{
/*if ((Sensor[0]<(MAX[0] - RES))&&(Sensor[1]<(MAX[1] - RES))&&(Sensor[2]<(MAX[2] - RES))&&(Sensor[3]<(MAX[3] - RES))) { mode0 = true; time0 = millis(); Dynamixel_crazy(); leds_white_on(); led_rgb(0, 4095, 0);}
else{
if (Sensor[0]<(MAX[0] - RES)) { mode1 = true; time1 = millis(); Dynamixel.turn(din[1],LEFT,map(Sensor[0], MAX[0] , LIMIT, 500, 1020)); led_white(1,4095); led_rgb(0, 0, 4095);}
if (Sensor[1]<(MAX[1] - RES)) { mode2 = true; time2 = millis(); Dynamixel.turn(din[3],LEFT,map(Sensor[1], MAX[1] , LIMIT, 500, 1020)); led_white(3,4095); led_rgb(4095, 0, 4095);}
if (Sensor[2]<(MAX[2] - RES)) { mode3 = true; time3 = millis(); Dynamixel.turn(din[4],LEFT,map(Sensor[2], MAX[2] , LIMIT, 500, 1000)); led_white(4,4095); led_rgb(0, 4095, 4095);}
if (Sensor[3]<(MAX[3] - RES)) { mode4 = true; time4 = millis(); Dynamixel.turn(din[6],LEFT,map(Sensor[3], MAX[3] , LIMIT, 500, 1000)); led_white(6,4095); led_rgb(1000, 4095, 4095);}
} */
resetTimeOut();
if (Sensor[0]<(MAX[0] - RES)) { s0 = true; }
else {s0 = false; isTimeOut[0] = true;}
if (Sensor[1]<(MAX[1] - RES)) { s1 = true; }
else {s1 = false; isTimeOut[1] = true;}
if (Sensor[2]<(MAX[2] - RES)) { s2 = true; }
else {s2 = false; isTimeOut[2] = true;}
if (Sensor[3]<(MAX[3] - RES)) { s3 = true; }
else {s3 = false; isTimeOut[3] = true;}
/////////////////
// ON
////////////////
if (s0 && !s1 && !s2 && !s3) { mode[0] = true; time[0] = millis(); Dynamixel.turn(din[1],RIGTH,1020); led_white(1,4095); }
if (!s0 && s1 && !s2 && !s3) { mode[1] = true; time[1] = millis(); Dynamixel.turn(din[3],LEFT,1020); led_white(3,4095); }
if (!s0 && !s1 && s2 && !s3) { mode[2] = true; time[2] = millis(); Dynamixel.turn(din[4],LEFT,1020); led_white(4,4095); }
if (!s0 && !s1 && !s2 && s3) { mode[3] = true; time[3] = millis(); Dynamixel.turn(din[6],RIGTH,1020); led_white(6,4095); }
if (s0 && s1 && !s2 && !s3) {
mode[4] = true; time[4] = millis();
Dynamixel.turn(din[1],RIGTH,1020);
Dynamixel.turn(din[3],LEFT,1020);
Dynamixel.turn(din[5],RIGTH,1020);
led_white(1,4095);
led_white(3,4095);
led_white(5,4095);
}
if (s0 && !s1 && s2 && !s3) {
mode[5] = true; time[5] = millis();
Dynamixel.turn(din[1],RIGTH,1020);
Dynamixel.turn(din[2],LEFT,1020);
Dynamixel.turn(din[5],RIGTH,1020);
led_white(1,4095);
led_white(2,4095);
led_white(5,4095);
}
if (s0 && !s1 && !s2 && s3) {
mode[6] = true; time[6] = millis();
Dynamixel.turn(din[1],RIGTH,1020);
Dynamixel.turn(din[4],LEFT,1020);
Dynamixel.turn(din[6],LEFT,1020);
led_white(1,4095);
led_white(4,4095);
led_white(6,4095);
}
if (s0 && s1 && s2 && !s3) {
mode[7] = true; time[7] = millis();
Dynamixel.turn(din[1],RIGTH,1020);
Dynamixel.turn(din[3],LEFT,1020);
Dynamixel.turn(din[5],RIGTH,1020);
led_white(1,4095);
led_white(3,4095);
led_white(5,4095);
}
if (s0 && !s1 && s2 && s3) {
mode[8] = true; time[8] = millis();
Dynamixel.turn(din[1],RIGTH,1020);
Dynamixel.turn(din[2],LEFT,1020);
Dynamixel.turn(din[4],LEFT,1020);
Dynamixel.turn(din[6],LEFT,1020);
led_white(1,4095);
led_white(2,4095);
led_white(4,4095);
led_white(6,4095);
}
if (s0 && s1 && s2 && s3) {
mode[9] = true; time[9] = millis();
Dynamixel.turn(din[1],RIGTH,1020);
Dynamixel.turn(din[2],LEFT,1020);
Dynamixel.turn(din[3],RIGTH,1020);
Dynamixel.turn(din[4],LEFT,1020);
Dynamixel.turn(din[5],RIGTH,1020);
Dynamixel.turn(din[6],LEFT,1020);
led_white(1,4095);
led_white(2,4095);
led_white(3,4095);
led_white(4,4095);
led_white(5,4095);
led_white(6,4095);
}
if (!s0 && s1 && s2 && !s3) {
mode[10] = true; time[10] = millis();
Dynamixel.turn(din[3],LEFT,1020);
Dynamixel.turn(din[4],LEFT,1020);
led_white(3,4095);
led_white(4,4095);
}
if (!s0 && s1 && !s2 && s3) {
mode[11] = true; time[11] = millis();
Dynamixel.turn(din[3],LEFT,1020);
Dynamixel.turn(din[5],RIGTH,1020);
Dynamixel.turn(din[6],LEFT,1020);
led_white(3,4095);
led_white(5,4095);
led_white(6,4095);
}
if (!s0 && s1 && s2 && s3) {
mode[12] = true; time[12] = millis();
Dynamixel.turn(din[2],LEFT,1020);
Dynamixel.turn(din[4],LEFT,1020);
Dynamixel.turn(din[5],RIGTH,1020);
Dynamixel.turn(din[6],LEFT,1020);
led_white(2,4095);
led_white(4,4095);
led_white(5,4095);
led_white(6,4095);
}
if (!s0 && !s1 && s2 && s3) {
mode[13] = true; time[13] = millis();
Dynamixel.turn(din[2],LEFT,1020);
Dynamixel.turn(din[3],LEFT,1020);
Dynamixel.turn(din[5],RIGTH,1020);
Dynamixel.turn(din[6],LEFT,1020);
led_white(2,4095);
led_white(3,4095);
led_white(5,4095);
led_white(6,4095);
}
/////////////////
// OFF
////////////////
if (((millis()- timeStop)>500)&&(modeStop)) { modeStop = false; leds_white_off(); Dynamixel_stop();}
if (((millis()- time[0])>500)&&(mode[0])) { mode[0] = false; led_white(1,0); Dynamixel.turn(din[1],LEFT,0);}
if (((millis()- time[1])>500)&&(mode[1])) { mode[1] = false; led_white(3,0); Dynamixel.turn(din[3],LEFT,0);}
if (((millis()- time[2])>500)&&(mode[2])) { mode[2] = false; led_white(4,0); Dynamixel.turn(din[4],LEFT,0);}
if (((millis()- time[3])>500)&&(mode[3])) { mode[3] = false; led_white(6,0); Dynamixel.turn(din[6],LEFT,0);}
if (((millis()- time[4])>500)&&(mode[4])) {
mode[4] = false;
Dynamixel.turn(din[1],RIGTH,0);
Dynamixel.turn(din[3],LEFT,0);
Dynamixel.turn(din[5],RIGTH,0);
led_white(1,0);
led_white(3,0);
led_white(5,0);
}
if (((millis()- time[5])>500)&&(mode[5])) {
mode[5] = false;
Dynamixel.turn(din[1],RIGTH,0);
Dynamixel.turn(din[2],LEFT,0);
Dynamixel.turn(din[5],RIGTH,0);
led_white(1,0);
led_white(2,0);
led_white(5,0);
}
if (((millis()- time[6])>500)&&(mode[6])) {
mode[6] = false;
Dynamixel.turn(din[1],RIGTH,0);
Dynamixel.turn(din[4],LEFT,0);
Dynamixel.turn(din[6],LEFT,0);
led_white(1,0);
led_white(4,0);
led_white(6,0);
}
if (((millis()- time[7])>500)&&(mode[7])) {
mode[7] = false;
Dynamixel.turn(din[1],RIGTH,0);
Dynamixel.turn(din[3],LEFT,0);
Dynamixel.turn(din[5],RIGTH,0);
led_white(1,0);
led_white(3,0);
led_white(5,0);
}
if (((millis()- time[8])>500)&&(mode[8])) {
mode[8] = false;
Dynamixel.turn(din[1],RIGTH,0);
Dynamixel.turn(din[2],LEFT,0);
Dynamixel.turn(din[4],LEFT,0);
Dynamixel.turn(din[6],LEFT,0);
led_white(1,0);
led_white(2,0);
led_white(4,0);
led_white(6,0);
}
if (((millis()- time[9])>500)&&(mode[9])) {
mode[9] = false;
Dynamixel.turn(din[1],RIGTH,0);
Dynamixel.turn(din[2],LEFT,0);
Dynamixel.turn(din[3],RIGTH,0);
Dynamixel.turn(din[4],LEFT,0);
Dynamixel.turn(din[5],RIGTH,0);
Dynamixel.turn(din[6],LEFT,0);
led_white(1,0);
led_white(2,0);
led_white(3,0);
led_white(4,0);
led_white(5,0);
led_white(6,0);
}
if (((millis()- time[10])>500)&&(mode[10])) {
mode[10] = false;
Dynamixel.turn(din[3],LEFT,0);
Dynamixel.turn(din[4],LEFT,0);
led_white(3,0);
led_white(4,0);
}
if (((millis()- time[11])>500)&&(mode[11])) {
mode[11] = false;
Dynamixel.turn(din[3],LEFT,0);
Dynamixel.turn(din[5],RIGTH,0);
Dynamixel.turn(din[6],LEFT,0);
led_white(3,0);
led_white(5,0);
led_white(6,0);
}
if (((millis()- time[12])>500)&&(mode[12])) {
mode[12] = false;
Dynamixel.turn(din[2],LEFT,0);
Dynamixel.turn(din[4],LEFT,0);
Dynamixel.turn(din[5],RIGTH,0);
Dynamixel.turn(din[6],LEFT,0);
led_white(2,0);
led_white(4,0);
led_white(5,0);
led_white(6,0);
}
if (((millis()- time[13])>500)&&(mode[13])) {
mode[13] = false;
Dynamixel.turn(din[2],LEFT,0);
Dynamixel.turn(din[3],LEFT,0);
Dynamixel.turn(din[5],RIGTH,0);
Dynamixel.turn(din[6],LEFT,0);
led_white(2,0);
led_white(3,0);
led_white(5,0);
led_white(6,0);
}
//------ TIMEOUT
if(isTimeOut[0] && isTimeOut[1] && isTimeOut[2] && isTimeOut[3]) { timeOut ++; }
else { resetTimeOut(); timeOut = 0;}
if (s0||s1||s2||s3||(!digitalRead(18))) {spam_on(); time_spam = millis();}
else if (((millis()-time_spam)>=600000)) spam_off();
if(timeOut>10){
reclama();
timeOut = 0;
}
}
}
void resetTimeOut(){
isTimeOut[0] = false;
isTimeOut[1] = false;
isTimeOut[2] = false;
isTimeOut[3] = false;
}
int modeTimeOut = 1;
void reclama(){
if(modeTimeOut==1){
for (int k = 0; k<4; k++){
for (byte i = 1; i<7; i++){
led_white(i, 4095);
delay(100);
}
for (byte i = 1; i<7; i++){
led_white(i, 0);
delay(100);
}
}
}
if(modeTimeOut==2){
for (int k = 1; k<7; k++){
Dynamixel.turn(din[k],LEFT,1020);
led_white((byte)k, 4095);
delay(500);
//Dynamixel.turn(din[k],LEFT,0);
}
for (int k = 1; k<7; k++){
Dynamixel.turn(din[k],LEFT,0);
led_white((byte)k, 0);
delay(500);
//Dynamixel.turn(din[k],LEFT,0);
}
}
if(modeTimeOut==3){
for (int k = 0; k<4; k++){
for (byte i = 6; i>0; i--){
led_white(i, 4095);
delay(100);
}
for (byte i = 6; i>0; i--){
led_white(i, 0);
delay(100);
}
}
}
if(modeTimeOut==4){
for (int k = 1; k<7; k++){
Dynamixel.turn(din[k],RIGTH,1020);
led_white((byte)k, 4095);
delay(500);
//Dynamixel.turn(din[k],LEFT,0);
}
for (int k = 1; k<7; k++){
Dynamixel.turn(din[k],RIGTH,0);
led_white((byte)k, 0);
delay(500);
//Dynamixel.turn(din[k],LEFT,0);
}
}
modeTimeOut++;
if(modeTimeOut>4) modeTimeOut=1;
}
/*
void marcha(boolean M1, boolean M2, boolean M3, boolean M4, boolean M5, boolean M6, String M1_dir, String M2_dir, String M3_dir, String M4_dir, String M5_dir, String M6_dir)
{
}*/
Reference in a new issue View git blame Copy permalink