// read model file void readModelFile(double *pi, double **A, double **B, int N, int M, int T, char **alphabet, FILE *filename ) { int N1, M1, T1; char str[80]; //FILE *in = fopen(filename, "r"); if (filename != NULL) { fscanf(filename, "N=%d, M=%d, T=%d\n", &N1, &M1, &T1); if (N1 != N || M1 != M) { fprintf(stderr, "\nN, or M is not the same parameters!\n\n"); exit(0); } fgets(str, sizeof(str), filename); //I: readPi(pi, N, filename); while (strstr(str, "A:")==NULL) fgets(str, sizeof(str), filename); //A: readA(A, N, filename); while (strstr(str, "B:")==NULL) fgets(str, sizeof(str), filename); //B: readBT(B, N, M, alphabet, filename); } //fclose(filename); }
void loop() { Serial1.print('r'); // request data from droid char bt = readBT(); if(bt == CMD_STOP) fullStop(); else if(bt == CMD_DRIVE) { if(status == CMD_STOP) { status = CMD_DRIVE; digitalWrite(STANDBY, HIGH); digitalWrite(STATUS_LED, HIGH); // indicate status } x = readBT(); y = readBT(); z = readBT(); int l = z + y, r = z - y; int L = (l < 0 ? -l : l)<<1; int R = (r < 0 ? -r : r)<<1; L = L > 255 ? 255 : L < 0 ? 0 : L; R = R > 255 ? 255 : R < 0 ? 0 : R; setMotor(MOTA, -l > 0 ? FORWARD : BACKWARD, L); setMotor(MOTB, -r > 0 ? FORWARD : BACKWARD, R); } }
void setup() { pinMode( SPDA , OUTPUT ); pinMode( SPDB , OUTPUT ); pinMode( STANDBY , OUTPUT ); pinMode( STATUS_LED , OUTPUT ); pinMode( DIR1A , OUTPUT ); pinMode( DIR2A , OUTPUT ); pinMode( DIR1B , OUTPUT ); pinMode( DIR2B , OUTPUT ); myservo.attach(HAXXXOR); // attaches the servo pin initservo(); fullStop(); //init's all previous outputs Serial.begin(USBSERIAL); // init USB Serial (console) Serial1.begin(BTGPSRATE); // init pin0/1 Serial (bluetooth) delay(1234); setupBT(3210); while(readBT() != CMD_STOP); // safety frist? ¯\(°_o)/¯ I DUNNO LOL }