mirror of
https://github.com/jjrobots/B-ROBOT_EVO2.git
synced 2026-03-03 00:24:02 +01:00
B-ROBOT EVO2
First upload: code, electronics...
This commit is contained in:
JJROBOTS
304
Arduino/BROBOT_EVO2/OSC.ino
Normal file
304
Arduino/BROBOT_EVO2/OSC.ino
Normal file
@@ -0,0 +1,304 @@
|
||||
// BROBOT EVO 2 by JJROBOTS
|
||||
// SELF BALANCE ARDUINO ROBOT WITH STEPPER MOTORS
|
||||
// License: GPL v2
|
||||
// OSC functions (OSC = Open Sound Control protocol)
|
||||
|
||||
// OSC Messages read: OSC: /page/command parameters
|
||||
// FADER (1,2,3,4) Ex: /1/fader1 f, XXXX => lenght:20, Param: float (0.0-1.0)
|
||||
// XY (1,2) Ex: /1/xy1 f,f, XXXXXXXX => length: 24 Params: float,float (0.0-1.0)
|
||||
// PUSH (1,2,3,4) Ex: /1/push1 f, XXXX => length:20 Param: float
|
||||
// TOGGLE (1,2,3,4) Ex: /1/toggle1 f, XXXX => length:20 Param: float
|
||||
// MOVE Ex: /1/m XXXX XXXX XXXX => length:16 Params: speed, steps1, steps2 (all float)
|
||||
//
|
||||
// OSC Message send:
|
||||
// string to send + param (float)[last 4 bytes]
|
||||
|
||||
|
||||
// for DEBUG uncomment this lines...
|
||||
//#define OSCDEBUG 3
|
||||
|
||||
|
||||
char UDPBuffer[8]; // input message buffer
|
||||
|
||||
// OSC message internal variables
|
||||
unsigned char OSCreadStatus;
|
||||
unsigned char OSCreadCounter;
|
||||
unsigned char OSCreadNumParams;
|
||||
unsigned char OSCcommandType;
|
||||
unsigned char OSCtouchMessage;
|
||||
|
||||
|
||||
// ------- OSC functions -----------------------------------------
|
||||
|
||||
// Aux functions
|
||||
float OSC_extractParamFloat(uint8_t pos) {
|
||||
union {
|
||||
unsigned char Buff[4];
|
||||
float d;
|
||||
} u;
|
||||
|
||||
u.Buff[0] = (unsigned char)UDPBuffer[pos];
|
||||
u.Buff[1] = (unsigned char)UDPBuffer[pos + 1];
|
||||
u.Buff[2] = (unsigned char)UDPBuffer[pos + 2];
|
||||
u.Buff[3] = (unsigned char)UDPBuffer[pos + 3];
|
||||
return (u.d);
|
||||
}
|
||||
|
||||
int16_t OSC_extractParamInt(uint8_t pos) {
|
||||
union {
|
||||
unsigned char Buff[2];
|
||||
int16_t d;
|
||||
} u;
|
||||
|
||||
u.Buff[1] = (unsigned char)UDPBuffer[pos];
|
||||
u.Buff[0] = (unsigned char)UDPBuffer[pos + 1];
|
||||
return (u.d);
|
||||
}
|
||||
|
||||
|
||||
void OSC_init()
|
||||
{
|
||||
OSCreadStatus = 0;
|
||||
OSCreadCounter = 0;
|
||||
OSCreadNumParams = 0;
|
||||
OSCcommandType = 0;
|
||||
OSCfader[0] = 0.5;
|
||||
OSCfader[1] = 0.5;
|
||||
OSCfader[2] = 0.5;
|
||||
OSCfader[3] = 0.5;
|
||||
}
|
||||
|
||||
void OSC_MsgSend(char *c, unsigned char msgSize, float p)
|
||||
{
|
||||
uint8_t i;
|
||||
union {
|
||||
unsigned char Buff[4];
|
||||
float d;
|
||||
} u;
|
||||
|
||||
// We copy the param in the last 4 bytes
|
||||
u.d = p;
|
||||
c[msgSize - 4] = u.Buff[3];
|
||||
c[msgSize - 3] = u.Buff[2];
|
||||
c[msgSize - 2] = u.Buff[1];
|
||||
c[msgSize - 1] = u.Buff[0];
|
||||
for (i = 0; i < msgSize; i++)
|
||||
{
|
||||
Serial1.write((uint8_t)c[i]);
|
||||
//Serial.write((uint8_t)c[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void OSC_MsgRead()
|
||||
{
|
||||
uint8_t i;
|
||||
uint8_t tmp;
|
||||
float value;
|
||||
float value2;
|
||||
|
||||
// New bytes available to process?
|
||||
if (Serial1.available() > 0) {
|
||||
//Serial.print("B:");
|
||||
//Serial.println(Serial1_available());
|
||||
// We rotate the Buffer (we could implement a ring buffer in future)
|
||||
for (i = 7; i > 0; i--) {
|
||||
UDPBuffer[i] = UDPBuffer[i - 1];
|
||||
}
|
||||
UDPBuffer[0] = Serial1.read();
|
||||
Serial.print(UDPBuffer[0]);
|
||||
#ifdef OSCDEBUG3
|
||||
Serial.print(UDPBuffer[0]);
|
||||
#endif
|
||||
// We look for an OSC message start like /x/
|
||||
if ((UDPBuffer[0] == '/') && (UDPBuffer[2] == '/') && ((UDPBuffer[1] == '1') || (UDPBuffer[1] == '2'))) {
|
||||
if (OSCreadStatus == 0) {
|
||||
OSCpage = UDPBuffer[1] - '0'; // Convert page to int
|
||||
OSCreadStatus = 1;
|
||||
OSCtouchMessage = 0;
|
||||
//Serial.print("$");
|
||||
#ifdef OSCDEBUG3
|
||||
Serial.println();
|
||||
#endif
|
||||
}
|
||||
else {
|
||||
Serial.println("!ERR:osc");
|
||||
OSCreadStatus = 1;
|
||||
}
|
||||
return;
|
||||
} else if (OSCreadStatus == 1) { // looking for the message type
|
||||
// Fadder /1/fader1 ,f xxxx
|
||||
if ((UDPBuffer[3] == 'd') && (UDPBuffer[2] == 'e') && (UDPBuffer[1] == 'r')) {
|
||||
OSCreadStatus = 2; // Message type detected
|
||||
OSCreadCounter = 11; // Bytes to read the parameter
|
||||
OSCreadNumParams = 1; // 1 parameters
|
||||
OSCcommandType = UDPBuffer[0] - '0';
|
||||
#ifdef OSCDEBUG2
|
||||
Serial.print("$FAD1");
|
||||
Serial.print(OSCcommandType);
|
||||
Serial.print("$");
|
||||
#endif
|
||||
return;
|
||||
} // end fadder
|
||||
// MOVE message
|
||||
if ((UDPBuffer[3] == 'o') && (UDPBuffer[2] == 'v') && (UDPBuffer[1] == 'e')) {
|
||||
OSCreadStatus = 2; // Message type detected
|
||||
OSCreadCounter = 8; // Bytes to read the parameters
|
||||
OSCreadNumParams = 3; // 3 parameters
|
||||
OSCcommandType = 40;
|
||||
#ifdef OSCDEBUG2
|
||||
Serial.print("$MOVE:");
|
||||
#endif
|
||||
return;
|
||||
} // End MOVE message
|
||||
// XY message
|
||||
if ((UDPBuffer[2] == 'x') && (UDPBuffer[1] == 'y')) {
|
||||
OSCreadStatus = 2; // Message type detected
|
||||
OSCreadCounter = 14; // Bytes to read the parameters
|
||||
OSCreadNumParams = 2; // 2 parameters
|
||||
OSCcommandType = 10 + (UDPBuffer[0] - '0');
|
||||
return;
|
||||
} // End XY message
|
||||
// Push message
|
||||
if ((UDPBuffer[3] == 'u') && (UDPBuffer[2] == 's') && (UDPBuffer[1] == 'h')) {
|
||||
OSCreadStatus = 2; // Message type detected
|
||||
OSCreadCounter = 10; // Bytes to read the parameter
|
||||
OSCreadNumParams = 1; // 1 parameters
|
||||
OSCcommandType = 20 + (UDPBuffer[0] - '1');
|
||||
//Serial.println(commandType);
|
||||
#ifdef OSCDEBUG2
|
||||
Serial.print("$P"):
|
||||
Serial.print(UDPBuffer[0] - '1');
|
||||
Serial.print(":");
|
||||
#endif
|
||||
return;
|
||||
} // end push
|
||||
// Toggle message
|
||||
if ((UDPBuffer[3] == 'g') && (UDPBuffer[2] == 'l') && (UDPBuffer[1] == 'e')) {
|
||||
OSCreadStatus = 2; // Message type detected
|
||||
OSCreadCounter = 10; // Bytes to read the parameter
|
||||
OSCreadNumParams = 1; // 1 parameters
|
||||
OSCcommandType = 30 + (UDPBuffer[0] - '1');
|
||||
//Serial.println(commandType);
|
||||
#ifdef OSCDEBUG2
|
||||
Serial.print("$T"):
|
||||
Serial.print(UDPBuffer[0] - '1');
|
||||
Serial.print(":");
|
||||
#endif
|
||||
return;
|
||||
} // end toggle
|
||||
} else if (OSCreadStatus == 2) {
|
||||
if ((UDPBuffer[1] == '/') && (UDPBuffer[0] == 'z')) { // Touch up message? (/z) [only on page1]
|
||||
if ((OSCpage == 1) && (OSCcommandType <= 2)) { // Touchup message only on Fadder1 and Fadder2
|
||||
OSCtouchMessage = 1;
|
||||
}
|
||||
else {
|
||||
OSCtouchMessage = 0;
|
||||
OSCreadStatus = 0; //Finish
|
||||
}
|
||||
} // Touch message(/z)
|
||||
OSCreadCounter--; // Reading counter until we reach the Parameter position
|
||||
if (OSCreadCounter <= 0) {
|
||||
OSCreadStatus = 0;
|
||||
OSCnewMessage = 1;
|
||||
//Serial.println(value);
|
||||
switch (OSCcommandType) {
|
||||
case 1:
|
||||
value = OSC_extractParamFloat(0);
|
||||
OSCfader[0] = value;
|
||||
if ((OSCtouchMessage) && (value == 0)) {
|
||||
OSCfader[0] = 0.5;
|
||||
//Serial.println("TOUCH_X");
|
||||
OSC_MsgSend("/1/fader1\0\0\0,f\0\0\0\0\0\0", 20, 0.5);
|
||||
}
|
||||
#ifdef OSCDEBUG
|
||||
Serial.print("$F1:");
|
||||
Serial.println(OSCfadder[0]);
|
||||
#endif
|
||||
break;
|
||||
case 2:
|
||||
value = OSC_extractParamFloat(0);
|
||||
OSCfader[1] = value;
|
||||
if ((OSCtouchMessage) && (value == 0)) {
|
||||
OSCfader[1] = 0.5;
|
||||
//Serial.println("TOUCH_Y");
|
||||
OSC_MsgSend("/1/fader2\0\0\0,f\0\0\0\0\0\0", 20, 0.5);
|
||||
}
|
||||
#ifdef OSCDEBUG
|
||||
Serial.print("$F2:");
|
||||
Serial.println(OSCfadder[1]);
|
||||
#endif
|
||||
break;
|
||||
case 3:
|
||||
OSCfader[2] = OSC_extractParamFloat(0);
|
||||
#ifdef OSCDEBUG
|
||||
Serial.print("$F3:");
|
||||
Serial.println(OSCfadder[2]);
|
||||
#endif
|
||||
break;
|
||||
case 4:
|
||||
OSCfader[3] = OSC_extractParamFloat(0);
|
||||
#ifdef OSCDEBUG
|
||||
Serial.print("$F4:");
|
||||
Serial.println(OSCfadder[3]);
|
||||
#endif
|
||||
break;
|
||||
case 11:
|
||||
OSCxy1_x = OSC_extractParamFloat(0);
|
||||
OSCxy1_y = OSC_extractParamFloat(4);
|
||||
#ifdef OSCDEBUG
|
||||
Serial.print("$XY1:");
|
||||
Serial.print(OSCxy1_x);
|
||||
Serial.print(",");
|
||||
Serial.println(OSCxy1_y);
|
||||
#endif
|
||||
break;
|
||||
case 12:
|
||||
OSCxy2_x = OSC_extractParamFloat(0);
|
||||
OSCxy2_y = OSC_extractParamFloat(4);
|
||||
#ifdef OSCDEBUG
|
||||
Serial.print("$XY2:");
|
||||
Serial.print(OSCxy2_x);
|
||||
Serial.print(",");
|
||||
Serial.println(OSCxy2_y);
|
||||
#endif
|
||||
break;
|
||||
case 40:
|
||||
// MOVE
|
||||
OSCmove_mode = 1;
|
||||
OSCmove_speed = OSC_extractParamInt(4);
|
||||
OSCmove_steps1 = OSC_extractParamInt(2);
|
||||
OSCmove_steps2 = OSC_extractParamInt(0);
|
||||
#ifdef OSCDEBUG
|
||||
Serial.print("$MOVE:");
|
||||
Serial.print(OSCmove_speed);
|
||||
Serial.print(",");
|
||||
Serial.print(OSCmove_steps1);
|
||||
Serial.print(",");
|
||||
Serial.println(OSCmove_steps2);
|
||||
#endif
|
||||
break;
|
||||
|
||||
default:
|
||||
// Push y toggle
|
||||
value = OSC_extractParamFloat(0);
|
||||
if ((OSCcommandType >= 20) && (OSCcommandType < 25))
|
||||
{
|
||||
if (value == 0)
|
||||
OSCpush[OSCcommandType - 20] = 0;
|
||||
else
|
||||
OSCpush[OSCcommandType - 20] = 1;
|
||||
}
|
||||
if ((OSCcommandType >= 30) && (OSCcommandType < 35))
|
||||
{
|
||||
if (value == 0)
|
||||
OSCtoggle[OSCcommandType - 30] = 0;
|
||||
else
|
||||
OSCtoggle[OSCcommandType - 30] = 1;
|
||||
}
|
||||
break;
|
||||
} // switch
|
||||
} // if (OSCRead_counter<=0)
|
||||
} // if (OSCread_status==2)
|
||||
} // end Serial.available()
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user