Demo2

////////////////////////////////////////////////////////////////////////
// Team 11  - ExoHand
// 
////////////////////////////////////////////////////////////////////////
#include <Servo.h>             //Servo library
Servo servo_left;        //initialize a servo object for the connected servo  
Servo servo_right;                
   
//GPIO13 CONNECTION LED STATUS
#define LED 13              //LED STATUS
#define BAUDRATE 57600
#define DEBUGOUTPUT 0

//GPIO 9 AND 10 LED OUTPUT
#define FOCUSLED 9                
#define FOCUSLED2 10

#define powercontrol 10

// 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;

//////////////////////////
// Microprocessor Setup //
//////////////////////////
void setup() {

  servo_left.attach(7);                   //SERVO IS ATTACHED TO GPIO 7 THIS MOVES PROPELLER LEFT OR CLOCKWISE
  servo_right.attach(7);                  //SERVO IS ATTACHED TO GPIO7 THIS MOVES PROPELLER RIGHT OR CLOCKWISE
  pinMode(FOCUSLED, OUTPUT);              //YELLOW LED INDICATES LOWER ATTENTION VALUES
  pinMode(FOCUSLED2, OUTPUT);             //GREEN LED INDICATE HIGHER ATTENTION VALUES
  pinMode(LED, OUTPUT);                   //LED STATUS FOR INTERCONNECTION OF EEG AND MICROCONTROLLER
  
  Serial.begin(BAUDRATE);           // USB

}

////////////////////////////////
// Read data from Serial UART //
////////////////////////////////
byte ReadOneByte() {
  int ByteRead;

  while(!Serial.available());
  ByteRead = Serial.read();

#if DEBUGOUTPUT  
  Serial.print((char)ByteRead);   // echo the same byte out the USB serial (for debug purposes)
#endif

  return ByteRead;
}

/////////////
//MAIN LOOP//
/////////////
void loop() {


  // Look for sync bytes
  if(ReadOneByte() == 170) {
    if(ReadOneByte() == 170) {

      payloadLength = ReadOneByte();
      if(payloadLength > 169)                           //Payload length can not be greater than 169
          return;

      generatedChecksum = 0;        
      for(int i = 0; i < payloadLength; i++) {  
        payloadData[i] = ReadOneByte();                //Read payload into memory
        generatedChecksum += payloadData[i];
      }   

      checksum = ReadOneByte();                        //Read checksum byte from stream      
      generatedChecksum = 255 - generatedChecksum;     //Take one's compliment of generated checksum

        if(checksum == generatedChecksum) {    

        poorQuality = 200;                            //WORST CASE NO COMMUNICATION BETWEEN EEG SENSOR AND MICROCONTROLLER
        attention = 0;
        meditation = 0;

        for(int i = 0; i < payloadLength; i++) {     // Parse the payload
          switch (payloadData[i]) {
          case 2:
            i++;            
            poorQuality = payloadData[i];
            bigPacket = true;            
            break;
          case 4:
            i++;
            attention = payloadData[i];                        
            break;
          case 5:
            i++;
            meditation = payloadData[i];
            break;
          //Adjusts smoothing
          case 0x80:
            i = i + 7;
            break;
         
          case 0x83:
            i = i + 23;      
            break;
          default:
            break;
          } // switch
        } // for loop


        // Serial Monitor
        //THE RED LED IS USED TO DETECT EEG SIGNAL QUALITY

        if(bigPacket) {
          if(poorQuality == 0)
            digitalWrite(LED, HIGH);
          else
            digitalWrite(LED, LOW);
          Serial.print(" PoorQuality: ");
          Serial.print(poorQuality, DEC);
          Serial.print(" Attention: ");
          Serial.print(attention);
          Serial.print(" Meditation: ");
          Serial.print(meditation, DEC);
          Serial.print(" Time since last packet: ");
          Serial.print(millis() - lastReceivedPacket, DEC);
          lastReceivedPacket = millis();
          Serial.print("\n");

          ///TRAINING Parameters for hand movements
if(poorQuality<10){
          switch(attention) {
          case 0 ... 29:                           //FOR ATTENTION VALUES THAT ARE BETWEEN AND INCLUDING 0 AND 29 WE DO NOT WANT ANYMOVEMENT BECAUSE OF NOISE VARIANCE DISTORTION
            servo_left.write(0);                   //RED LED LIGHT
            servo_right.write(0);
            digitalWrite(FOCUSLED, LOW);
            digitalWrite(FOCUSLED2, LOW);
            delay(1000);
            break;
          case 30 ... 49:                           //FOR CASES WHERE ATTENTION SPAN IS FROM 30 TO 49 THE SERVO MOTOR MOVES 23 DEGREES COUNTER CLOCK WISE OR RIGHT (MINIMUM ANGLE)
            servo_left.write(23);                   //YELLOW LED LIGHT
            servo_right.write(23);
            digitalWrite(FOCUSLED, HIGH);
            digitalWrite(FOCUSLED2, LOW);
            delay(1000);
            break;
          case 50 ... 75:                           //FOR CASES WHERE ATTENTION SPAN IS FROM 50 TO 75 THE SERVO MOTOR MOVES 45 DEGREES COUNTER CLOCK WISE OR RIGHT (MAXIMUM ANGLE)
            servo_left.write(55);                   //GREEN LED LIGHT
            servo_right.write(55);
            digitalWrite(FOCUSLED, LOW);
            digitalWrite(FOCUSLED2, HIGH);
            delay(1000);
            break;
          case 76 ... 100:
            servo_left.write(-45);                  //FOR CASES WHERE THE ATTENTION SPAN IS VERY HIGH THIS WILL SERVE AS A FEED BACK TO REDUCE DAMAGE TO THE FINGERS
            servo_right.write(-45);                 //NO LIGHTS
            digitalWrite(FOCUSLED, LOW);
            digitalWrite(FOCUSLED2, LOW);
            delay(1000);
            break;
          }                     
        }
      }     
        bigPacket = false;        
        } // Checksum Error
      } // end if else for checksum
    } // end if read 0xAA byte
  } // end if read 0xAA byte