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

536 lines
13 KiB
C++
Raw Permalink Blame History

// FALTA ENVIAR EN FORMATO OSC EL ARRANQUE DEL WIFLY
// ARDUINO PRO - 5V - 16MHz - ATmega 328
// http://arduino.cc/en/Guide/ArduinoPro
// FTDI drivers http://www.ftdichip.com/Drivers/VCP.htm
#include "WiFlyHQ_.h"
#include <Wire.h>
const char mySSID[] = "hangar_nau3";
const char myPassword[] = "m1cr0fug4s";
const char *IP = "172.26.0.255";
//const char mySSID[] = "gira_li";
//sll -- const char myPassword[] = "wifpanspermia";
//const char myPassword[] = "";
//const char *IP = "192.168.2.10"; // receiber computer ip
//const char *IP = "192.168.2.14"; // receiber computer ip
/*
const char mySSID[] = "xarxalab";
const char myPassword[] = "la$connexio$a$xarxalab";
const char *IP = "192.168.2.102";
*/
// const char *IP = "192.168.2.255"; // broadcast per a tots...
const uint16_t outPort = 8000;
const uint16_t localPort = 9000;
#define ADDRESS 0x61 // The address of the SD21
#define debug 0
#define bat A1
#define ledBat 18 //A0
#define ledDatos 4
#define releServos 7
#define batThreshold 136
#define SERVO1 0x3F // Address of first servo
#define SERVO2 0x40
#define SERVO3 0x41
#define SERVO4 0x42
#define SERVO5 0x43
#define SERVO6 0x44
#define SERVO7 0x45
#define SERVO8 0x46
#define SERVO9 0x47
#define SERVO10 0x48
#define VELSERVO1 0x00 // Address of first servo speed
#define VELSERVO2 0x03
#define VELSERVO3 0x06
#define VELSERVO4 0x09
#define VELSERVO5 0x0C
#define VELSERVO6 0x0F
#define VELSERVO7 0x12
#define VELSERVO8 0x15
#define VELSERVO9 0x18
#define VELSERVO10 0x1B
#define limit1Low 30
#define limit1High 100
#define center1 55
#define limit2Low 10
#define limit2High 75
#define center2 45
#define limit3Low 15
#define limit3High 80
#define center3 50
#define limit4Low 25
#define limit4High 95
#define center4 63
#define limit5Low 5
#define limit5High 65
#define center5 50
#define limit6Low 15
#define limit6High 80
#define center6 25
#define limit7Low 4
#define limit7High 100
#define center7 15
#define limit8Low 1
#define limit8High 100
#define center8 15
#define limit9Low 1
#define limit9High 90
#define center9 50
#define limit10Low 12
#define limit10High 100
#define center10 50
uint32_t count=0;
uint32_t error=0;
uint8_t tick=0;
uint8_t tempByte = 0;
uint8_t serialCount=0;
uint32_t lastSend = 0;
uint32_t lastEvent = 0;
uint32_t intValue1, intValue2, intValue3, intValue4;
#define MESSAGE_SIZE 12
unsigned char incomming_message[MESSAGE_SIZE] = { // Message template
'/', 's', '1', B0,
',', 'i', B0, B0,
B0 , B0 , B0, B0
};
char pingBat[MESSAGE_SIZE] = { // Message template
'/', 'b', 't', B0 ,
',', 'i', B0, B0,
B0 , B0 , B0, B0
};
char err[MESSAGE_SIZE] = { // Message template
'/', 'e', 'r', B0,
',', 'i', B0, B0,
B0 , B0 , B0, B0
};
WiFly wifly;
void setup()
{
char buf[32];
Wire.begin();
pinMode(ledDatos, OUTPUT); // LED DATOS
pinMode(releServos, OUTPUT); // RELE
pinMode(ledBat, OUTPUT); // LED BATERIA
Serial.begin(9600);
Serial1.begin(9600);
wifly.begin(&Serial1);
/* Join wifi network if not already associated */
if (!wifly.isAssociated()) {
/* Setup for UDP packets, sent automatically */
wifly.setIpProtocol(WIFLY_PROTOCOL_UDP);
wifly.setHost(IP, outPort); // Send UDP packet to this server and port
wifly.setPort(localPort); // listen in this local port
/* Setup the WiFly to connect to a wifi network */
wifly.setSSID(mySSID);
wifly.setPassphrase(myPassword);
//wifly.setKey(myKey); // modo WEP
wifly.enableDHCP();
wifly.join();
}
//wifly.setIP(const __FlashStringHelper *buf) // EN CASO DE USAR IP FIJA
//wifly.setNetmask(const char *buf)
Serial.print("MAC:");
Serial.println(wifly.getMAC(buf, sizeof(buf)));
Serial.print("IP: ");
Serial.println(wifly.getIP(buf, sizeof(buf)));
Serial.print("Netmask: ");
Serial.println(wifly.getNetmask(buf, sizeof(buf)));
Serial.print("Gateway: ");
Serial.println(wifly.getGateway(buf, sizeof(buf)));
Serial.print("LocalPort: ");
Serial.println(wifly.getPort());
Serial.print("HostPort: ");
Serial.println(wifly.getHostPort());
wifly.setDeviceID("CAP");
Serial.print("DeviceID: ");
Serial.println(wifly.getDeviceID(buf, sizeof(buf)));
Serial.println("CAP ready");
digitalWrite(releServos, HIGH); // ARRANCA LA PLACA DE SERVOS (RELE ON)
delay(100);
servoReset(); // RESETEAMOS SERVOS A POSICION CENTRAL
}
void loop()
{
if ((millis() - lastSend) > 30000) { // interrupccion cada 30s para enviar estado de bateria y actualizacion de led
digitalWrite(ledDatos, LOW);
long bateria = analogRead(bat)/4;
if( bateria < batThreshold) // Bat MAX 16V = 203
digitalWrite(ledBat, HIGH); // Bat 11V = 136
else // Bat min 10V = 125
digitalWrite(ledBat, LOW);
bateria = map(bateria, 130, 210, 0, 100);
if( bateria > 100 ) bateria = 100;
else if( bateria < 0 ) bateria = 0;
//pingBat[11] = (int)bateria;
for(int b=0 ; b<12 ; b++)
wifly.write(pingBat[b]);
lastSend = millis();
digitalWrite(ledDatos, HIGH);
}
if ((millis() - lastEvent) > 60000) // INTERRUPCION PARA PONER EN REPOSO LOS SERVOS TRAS 60 SEG DE INACTIVIDAD
{
digitalWrite(releServos, LOW);
}
checkIncomming(); // chequeamos los mensajes por wifi!!!
}
void SD21( byte CMD, byte inByte )
{
digitalWrite(ledDatos, LOW);
Wire.beginTransmission(ADDRESS);
Wire.write(CMD);
Wire.write(inByte); // Send a value of 0-255 to servo 1
Wire.endTransmission();
digitalWrite(ledDatos, HIGH);
}
void servoReset()
{
// SERVOS A POSICION CENTRAL EXCEPTO LA BOCA
/*
SD21(SERVO1, 85*255/180); // min:50 max:120 med:85
SD21(SERVO2, 96*255/180); // min:80 max:112 med:96
SD21(SERVO3, 90*255/180); // min:55 max:125 med:90
SD21(SERVO4, 80*255/180); // min:60 max:100 med:80
SD21(SERVO5, 31*255/180); // min:12 max:50 med:31
SD21(SERVO6, 101*255/180); // min:80 max:122 med:101
SD21(SERVO7, 65*255/180); // min:0 max:130 med:65
SD21(SERVO8, 117*255/180); // min:55 max:180 med:117
SD21(SERVO9, 50*255/180); // min:50 max:120 med:85 -> BOCA ARRANCA EN POSICION BAJA
SD21(SERVO10, 80*255/180); // min:45 max:115 med:80
*/
SD21(SERVO1, center1);
SD21(SERVO2, center2);
SD21(SERVO3, center3);
SD21(SERVO4, center4);
SD21(SERVO5, center5);
SD21(SERVO6, center6);
SD21(SERVO7, center7);
SD21(SERVO8, center8);
SD21(SERVO9, center9);
SD21(SERVO10, limit10High);
}
void moveServoOriginal(long servo, long pos, long limitDown, long limitHigh)
{
if(pos>100) pos = 100;
else if(pos<0) pos = 0;
if(pos > 0) // si es igual a 0 no se actualiza el motor!!!
SD21( servo, map(pos, 1, 100, limitDown*255/180, limitHigh*255/180) );
}
void moveServo(long servo, long pos, long limitDown, long limitHigh)
{
if(pos != 0) { // si es igual a 0 no se actualiza el motor!!!
if(pos > limitHigh) pos = limitHigh;
else if(pos < limitDown) pos = limitDown;
SD21( servo, pos );
}
}
void velServo(long servoVel, long vel)
{
if(vel>100) vel = 100;
else if(vel<0) vel = 0;
if(vel > 0) // si es igual a 0 no se actualiza el motor!!!
SD21( servoVel , map(vel, 1, 100, 1, 128 ) );
}
void checkIncomming() {
if(wifly.available() )
{
lastEvent = millis(); // inicializo contador de actividad!
digitalWrite(releServos, HIGH); // Activamos la placa de motores
digitalWrite(ledDatos, LOW); // led de datos ON
tempByte = wifly.read();
Serial.write(tempByte);
if (tempByte == '/')
{
serialCount = 0;
for(int j=0; j<MESSAGE_SIZE ; j++)
incomming_message[j] = '/0';
}
incomming_message[serialCount] = tempByte;
serialCount++;
if(serialCount>=12)
{
// Check OSC Message
if( (incomming_message[0] == '/') && ( incomming_message[1] == 'm' ) ) // MENSAJE DE CONTROL DE MOTOR
{
intValue4 = incomming_message[8];
intValue3 = incomming_message[9];
intValue2 = incomming_message[10];
intValue1 = incomming_message[11];
switch (incomming_message[2]) // COMANDO
{
case '1': // servos 1, 2, 3 y 4
moveServo(SERVO1, intValue1, limit1Low, limit1High);
moveServo(SERVO2, intValue2, limit2Low, limit2High);
moveServo(SERVO3, intValue3, limit3Low, limit3High);
moveServo(SERVO4, intValue4, limit4Low, limit4High);
if(debug)
{
wifly.print("s1: ");
wifly.println(intValue1);
wifly.print("s2: ");
wifly.println(intValue2);
wifly.print("s3: ");
wifly.println(intValue3);
wifly.print("s4: ");
wifly.println(intValue4);
}
break;
case '2':
moveServo(SERVO5, intValue1, limit5Low, limit5High);
moveServo(SERVO6, intValue2, limit6Low, limit6High);
moveServo(SERVO7, intValue3, limit7Low, limit7High);
moveServo(SERVO8, intValue4, limit8Low, limit8High);
if(debug)
{
wifly.print("s5: ");
wifly.println(intValue1);
wifly.print("s6: ");
wifly.println(intValue2);
wifly.print("s7: ");
wifly.println(intValue3);
wifly.print("s8: ");
wifly.println(intValue4);
}
break;
case '3':
moveServo(SERVO9, intValue1, limit9Low, limit9High);
moveServo(SERVO10, intValue2, limit10Low, limit10High);
if(debug)
{
wifly.print("s9: ");
wifly.println(intValue1);
wifly.print("s10: ");
wifly.println(intValue2);
}
break;
}
//Reseteamos contador y array despues de dato correcto
serialCount = 0;
for(int i=0; i<MESSAGE_SIZE ; i++)
incomming_message[i] = '/0';
}
else if( (incomming_message[0] == '/') && ( incomming_message[1] == 'v' ) ) // MENSAJE DE CONTROL DE VELOCIDAD
{
//intValue = (incomming_message[11] | incomming_message[10]<<8 | incomming_message[9]<<16 | incomming_message[8]<<24 );
intValue4 = incomming_message[8];
intValue3 = incomming_message[9];
intValue2 = incomming_message[10];
intValue1 = incomming_message[11];
switch (incomming_message[2])
{
case '1': // servos 1, 2, 3 y 4
velServo(VELSERVO1, intValue1);
velServo(VELSERVO2, intValue2);
velServo(VELSERVO3, intValue3);
velServo(VELSERVO4, intValue4);
if(debug)
{
wifly.print("v1: ");
wifly.println(intValue1);
wifly.print("v2: ");
wifly.println(intValue2);
wifly.print("v3: ");
wifly.println(intValue3);
wifly.print("v4: ");
wifly.println(intValue4);
}
break;
case '2':
velServo(VELSERVO5, intValue1);
velServo(VELSERVO6, intValue2);
velServo(VELSERVO7, intValue3);
velServo(VELSERVO8, intValue4);
if(debug)
{
wifly.print("v5: ");
wifly.println(intValue1);
wifly.print("v6: ");
wifly.println(intValue2);
wifly.print("v7: ");
wifly.println(intValue3);
wifly.print("v8: ");
wifly.println(intValue4);
}
break;
case '3':
velServo(VELSERVO9, intValue1);
velServo(VELSERVO10, intValue2);
if(debug)
{
wifly.print("v9: ");
wifly.println(intValue1);
wifly.print("v10: ");
wifly.println(intValue2);
}
break;
}
//Reseteamos contador y array despues de dato correcto
serialCount = 0;
for(int i=0; i<MESSAGE_SIZE ; i++)
incomming_message[i] = '/0';
}
else if( (incomming_message[0] == '/') && ( incomming_message[1] == 's' ) ) // MENSAJE DE STOP
{
intValue1 = incomming_message[11];
if (intValue1)
digitalWrite(releServos, HIGH);
else
digitalWrite(releServos, LOW);
}
else
{
error++;
if(debug){
err[11] = error;
for(int b=0 ; b<12 ; b++)
wifly.write(err[b]);
//wifly.print("Error = ");
//wifly.print(error);
//wifly.print(" ");
}
//Reseteamos contador y array
serialCount = 0;
for(int i=0; i<MESSAGE_SIZE ; i++)
incomming_message[i] = '/0';
}
}
digitalWrite(ledDatos, HIGH); //APAGAMOS LED DE DATOS
}
}
Reference in a new issue View git blame Copy permalink