mirror of
https://github.com/jjrobots/B-ROBOT_EVO2.git
synced 2026-02-20 03:11:28 +01:00
302 lines
9.0 KiB
C++
302 lines
9.0 KiB
C++
// 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 0
|
|
|
|
|
|
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) {
|
|
// 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();
|
|
#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(OSCfader[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(OSCfader[1]);
|
|
#endif
|
|
break;
|
|
case 3:
|
|
OSCfader[2] = OSC_extractParamFloat(0);
|
|
#ifdef OSCDEBUG
|
|
Serial.print("$F3:");
|
|
Serial.println(OSCfader[2]);
|
|
#endif
|
|
break;
|
|
case 4:
|
|
OSCfader[3] = OSC_extractParamFloat(0);
|
|
#ifdef OSCDEBUG
|
|
Serial.print("$F4:");
|
|
Serial.println(OSCfader[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()
|
|
}
|
|
|