#define bleSerial Serial1 #define itrSerial Serial4 const char* cmds[] = { "$$$", "+\r", "V\r", "GK\r", "GNR\r", // "S-,LDsRN4871\r", // "SS,C0\r", "R,1\r", // "hello\r" }; int idx = 0; void setup() { pinMode(28,OUTPUT); pinMode(7,OUTPUT); pinMode(6,INPUT); digitalWrite(28,HIGH); pinMode(3,OUTPUT); Serial.begin(9600); bleSerial.begin(115200,SERIAL_8N1); itrSerial.begin(9600); while(!Serial){} while(!bleSerial){} while(!itrSerial){} } int btnPrev = HIGH; char buff[256]; int bidx = 0; void loop() { int btnCurr = digitalRead(6); if (btnPrev == HIGH && btnCurr == LOW){ digitalWrite(7,HIGH); delay(500); const char* cmd = cmds[idx++]; int i = 0; while (1){ bleSerial.write(cmd[i]); Serial.write(cmd[i]); i++; if (cmd[i] == 0) break; } if (idx >= sizeof(cmds)/sizeof(cmds[0])){ idx = 0; } digitalWrite(7,LOW); } btnPrev = btnCurr; if (bleSerial.available() > 0) { char b = bleSerial.read(); char s[2] = {0}; sprintf(s,"%c",b); Serial.print(s); if (b == ')'){ buff[bidx] = b; int i; for (i = 0; i < 256; i++){ if (buff[i] == '('){ i++; break; } } int j = i; Serial.print("\nSENDING:"); while (1){ itrSerial.write(buff[j]); Serial.write(buff[j]); j++; if (buff[j] == ')'){ break; } } itrSerial.write('\n'); Serial.print("\n"); buff[bidx] = 0; if (buff[i] == 'm'){ int n = atoi(buff+i+1); int a = ((float)n/100.0) * 2250 + 350; int b = 20000-a; Serial.print("\nMOTOR:"); Serial.print(a); Serial.print(","); Serial.print(b); Serial.print("\n"); for (int i = 0; i < 30; i++){ digitalWrite(3,HIGH); delayMicroseconds(a); digitalWrite(3,LOW); delayMicroseconds(b); } } if (buff[i] == 'l'){ if (buff[i+1] == '1'){ digitalWrite(7,HIGH); }else if (buff[i+1] == '0'){ digitalWrite(7,LOW); } } bidx = 0; }else{ buff[bidx] = b; bidx++; } } }