hoverTeensy.ino

#define LEDPIN 13

void setup() {
  Serial1.begin(26315, SERIAL_9N1);
  Serial.begin(115200);
}

char c = ' ';
signed int sp=0;

void loop() {
  Serial.println(c);
  if(c == ' ') {
    sp=0;
  } else if(c == 'q') {
    sp -= 10;
  } else if(c == 'w') {
    sp += 10;
  } else if(c == '2') {
    sp += 100;
  }  else if(c == '1') {
    sp -= 100;
  }
  Serial.print("speed ");
  Serial.println(sp);
  Serial.print(" low byte ");
  Serial.print((sp & 0xFF), HEX);
  Serial.print(" high byte ");
  Serial.println((sp >> 8) & 0xFF, HEX);
  do {
      //    GYRO_FRAME_START,
      Serial1.write9bit(256);
      Serial1.write9bit(sp & 0xFF);
      Serial1.write9bit((sp >> 8) & 0xFF);
      Serial1.write9bit(sp & 0xFF);
      Serial1.write9bit((sp >> 8) & 0xFF);
       //GYRO_CONTACT_CLOSED_BYTE
      Serial1.write9bit(85);
      delayMicroseconds(300);
  } while(!Serial.available());
  c=Serial.read();
}