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