/* MindBullets.pde - Interface to trigger a modified ** Nerf Stampede to fire when the appropriate signals ** are received from a NeuroSky EEG headset. ** ** Several portions of this code are borrowed and ** modified from the NeuroSky Arduino project files. ** The rest is from Hex Machina. Permission is granted ** to modify and distribute for non-commercial, ** non-military purposes. ** ** (C) 2011 Hex Machina (except where noted) */ #define MACADDR 0013ef004ba7 #define BTBAUDRATE 57600 // Attention threshold 1-100 above which the fire command is sent #define FIRE_THRESHOLD 40 // Number below which we consider the signal to be good, 1-255. // Lower non-zero numbers denote better signal. #define POOR_QUALITY_THRESHOLD 200 // Set to 1 to program the BlueSMiRF to search automatically #define PROGRAM 0 // Set to 1 to have the BlueSMiRF search just once on start #define ONTHEFLY 0 // Set to 1 to enable pass-through terminal mode for querying // the BlueSMiRF #define TERMINAL 0 // Set to 0 to use NewSoftSerial--note this is not recommended // for gathering actual data from the EEG headset #define HARDSERIAL 1 // Increase non-zero values for increasing levels of debug info #define DEBUG 0 // Make sure you've downloaded and installed the // NewSoftSerial library prior to enabling #if HARDSERIAL == 0 #include #endif char mac[13] = "MACADDR"; char key[5] = "0000"; #if HARDSERIAL != 0 int RX = 0; int TX = 1; #else int RX = 2; int TX = 3; #endif int BTPWR = 7; int FIRE_READY = 8; int FIRE = 9; int ATT_LED = 10; int MED_LED = 11; int STATUS = 13; // The following definitions are pqart of the NeuroSky sample code // checksum variables byte generatedChecksum = 0; byte checksum = 0; int payloadLength = 0; byte payloadData[64] = {0}; byte poorQuality = 0; byte attention = 0; byte meditation = 0; // system variables long lastReceivedPacket = 0; boolean bigPacket = false; // End NeuroSky sample vars #if HARDSERIAL == 0 NewSoftSerial BTSerial(RX,TX); #endif void setup() { Serial.begin(57600); #if HARDSERIAL == 0 BTSerial.begin(BTBAUDRATE); #endif pinMode( RX, INPUT ); pinMode( TX, OUTPUT ); pinMode( STATUS, OUTPUT ); pinMode( BTPWR, OUTPUT ); pinMode( FIRE_READY, OUTPUT ); pinMode( FIRE, OUTPUT ); pinMode( ATT_LED, OUTPUT ); pinMode( MED_LED, OUTPUT ); digitalWrite( BTPWR, HIGH ); delay( 500 ); #if TERMINAL != 1 #if PROGRAM == 1 BTProgram(); #endif #if ONTHEFLY == 1 BTInit(); #endif #endif blink( 3, 200 ); #if DEBUG >= 1 Serial.println("Ready"); #endif } void loop() { #if TERMINAL == 1 && HARDSERIAL == 0 BTSerial.print("$$$"); while ( true ) { BluetoothConduit(); delay( 2 ); } #endif readNeuroValues(); if (attention >= FIRE_THRESHOLD ) fire(); } #if HARDSERIAL == 0 /* This passes commands from the Serial terminal in the ** Arduino IDE to the attached Bluetooth module and relays ** responses from the Bluteooth module back to the IDE. ** NewSoftSerial is required for this. */ void BluetoothConduit() { int count = 0; if (Serial.available()) { Serial.print("> "); while (Serial.available()) { count++; char a = Serial.read(); BTSerial.print(a); Serial.print(a); } BTSerial.print('\r'); BTSerial.flush(); Serial.println(); } delay( 20 ); count = 0; if (BTSerial.available()) { Serial.print("< "); while (BTSerial.available()) { count++; char a = BTSerial.read(); Serial.print(a); } Serial.println(); Serial.flush(); } } #endif /* Tries to manually connect to the MAC address, rather than ** programming the Bluetooth module. Good for one-shot solutions. */ void BTInit() { Serial.print( "$$$" ); delay( 150 ); Serial.flush(); Serial.print( "C," ); Serial.print( mac ); Serial.print( '\r' ); Serial.flush(); Serial.print( "---\r" ); delay( 150 ); #if DEBUG >= 1 Serial.println("Inited"); #endif } /* Programs the Bluetooth module to automatically go into ** Bluetooth master mode and scan for a given MAC address. ** After programming the Bluetooth module, the Arduino will ** have to be reprogrammed to interpret the data coming in. ** This is heavily modified from the NeuroSky sample code. ** (Portions (C) 2011 NeuroSky) */ void BTProgram() { #if HARDSERIAL == 0 BTSerial.print( "$$$" ); delay( 150 ); BTSerial.print( "SU,57\r" ); if ( !OKrcvd("CMD") ) { Serial.println("No SU"); return; } BTSerial.flush(); BTSerial.print( "SR," ); BTSerial.print( mac ); BTSerial.print( '\r' ); if ( !OKrcvd("AOK") ) { Serial.println("No SR"); return; } BTSerial.flush(); BTSerial.print( "SP," ); BTSerial.print( key ); BTSerial.print( '\r' ); if ( !OKrcvd("AOK") ) { Serial.println("No SP"); return; } BTSerial.flush(); BTSerial.print( "SM,3\r" ); if ( !OKrcvd("AOK") ) { Serial.println("No SM"); return; } BTSerial.flush(); BTSerial.print( "---\r" ); delay( 150 ); #if DEBUG >= 1 Serial.println("Inited"); #endif #else Serial.print( "$$$" ); delay( 150 ); Serial.print( "SU,57\r" ); if ( !OKrcvd("CMD") ) { Serial.println("No SU"); return; } Serial.flush(); Serial.print( "SR," ); Serial.print( mac ); Serial.print( '\r' ); if ( !OKrcvd("AOK") ) { Serial.println("No SR"); return; } Serial.flush(); Serial.print( "SP," ); Serial.print( key ); Serial.print( '\r' ); if ( !OKrcvd("AOK") ) { Serial.println("No SP"); return; } Serial.flush(); Serial.print( "SM,3\r" ); if ( !OKrcvd("AOK") ) { Serial.println("No SM"); return; } Serial.flush(); Serial.print( "---\r" ); delay( 150 ); #if DEBUG >= 1 Serial.println("Inited"); #endif #endif } /* Checks to make sure a given three-character response is ** received from the Bluetooth module. Heavily modified from ** NeuroSky sample code. (Portions (C) 2011 NeuroSky) */ boolean OKrcvd(char cmd[]) { int time = 0; #if HARDSERIAL != 0 while( Serial.available() < 3 && time < 1000 ) { time++; delay( 1 ); } #else while( BTSerial.available() < 3 && time < 1000 ) { time++; delay( 1 ); } #endif if ( time >= 1000 ) { digitalWrite( BTPWR, LOW ); Serial.println( "Timeout on msg" ); return false; } char str[3]; for ( int i = 0; i < 3; i++ ) #if HARDSERIAL != 0 str[ i ] = (char) Serial.read(); #else str[ i ] = (char) BTSerial.read(); #endif if (str[ 0 ] != cmd[ 0 ] || str[ 1 ] != cmd[ 1 ] || str[ 2 ] != cmd[ 2 ] ) { digitalWrite( BTPWR, LOW ); Serial.print( "Got wrong msg: " ); Serial.print( str ); Serial.print( ", expected " ); Serial.print( cmd ); Serial.println(); return false; } return true; } // A basic blink algorithm. void blink( int times, int duration ) { for ( int i = 0; i < times; i++ ) { digitalWrite( STATUS, HIGH ); delay( duration ); digitalWrite( STATUS, LOW ); delay( duration ); } } /* Reads in one byte from the chosen serial interface. ** Lifted almost directly from the NeuroSKy sample code. ** (Portions (C) 2011 NeuroSky) */ byte ReadOneByte() { int ByteRead; #if HARDSERIAL != 0 while(!Serial.available()); ByteRead = Serial.read(); #else while(!BTSerial.available()); ByteRead = BTSerial.read(); #endif return ByteRead; } /* Reads in data coming from the NeuroSky headset, checks ** for integrity, and then assigns values to the attention ** and meditation values if everything checks out. Debugging ** and LED status added by Hex Machina, all else is NeuroSky ** sample code. (Portions (C) 2011 NeuroSky) */ void readNeuroValues() { // Look for sync bytes if(ReadOneByte() == 170) { #if DEBUG >= 2 Serial.print("-"); #endif if(ReadOneByte() == 170) { #if DEBUG >= 2 Serial.println("+"); #endif payloadLength = ReadOneByte(); if(payloadLength > 169) //Payload length can not be greater than 169 { #if DEBUG >= 1 Serial.print( "Payload length excessive: " ); Serial.println( payloadLength ); #endif return; } generatedChecksum = 0; #if DEBUG >= 3 Serial.print( "Reading payload... " ); #endif for(int i = 0; i < payloadLength; i++) { payloadData[i] = ReadOneByte(); //Read payload into memory generatedChecksum += payloadData[i]; } #if DEBUG >= 3 Serial.println( "Done" ); #endif checksum = ReadOneByte(); //Read checksum byte from stream generatedChecksum = 255 - generatedChecksum; //Take one's compliment of generated checksum if(checksum == generatedChecksum) { poorQuality = POOR_QUALITY_THRESHOLD; attention = 0; meditation = 0; for(int i = 0; i < payloadLength; i++) { // Parse the payload switch (payloadData[i]) { case 2: i++; poorQuality = payloadData[i]; #if DEBUG >= 1 Serial.print( poorQuality, DEC ); Serial.print( ": " ); #endif bigPacket = true; break; case 4: i++; attention = payloadData[i]; break; case 5: i++; meditation = payloadData[i]; break; case 0x80: i = i + 3; break; case 0x83: i = i + 25; break; default: break; } // switch } // for loop bigPacket = false; } else { #if DEBUG >= 2 Serial.println("Bad checksum!"); #endif } // end if else for checksum #if DEBUG >= 1 if ( attention > 0 || meditation > 0 ) { Serial.print( attention, DEC ); Serial.print( ", " ); Serial.println( meditation, DEC ); } #endif // Adjust status LEDs analogWrite( ATT_LED, attention * 2 + 55 ); analogWrite( MED_LED, meditation * 2 + 55 ); } // end if read 0xAA byte } // end if read 0xAA byte } /* Fire the Nerf Stampede. Because of the two-relay system, ** the ground connection between the trigger and the motor ** must be broken first (FIRE_READY is brought high), and ** then the fire command is issued (FIRE is brought high). ** To stop, the ground connection between the trigger and ** motor must be re-enabled (FIRE_READY and FIRE both ** brought low simultaneously). */ void fire() { #if DEBUG >= 1 Serial.println( "Fire!" ); #endif digitalWrite( STATUS, HIGH ); digitalWrite( FIRE_READY, HIGH ); delay( 100 ); digitalWrite( FIRE, HIGH ); delay( 150 ); digitalWrite( FIRE_READY, LOW ); digitalWrite( FIRE, LOW ); digitalWrite( STATUS, LOW ); }