Serial functions and general firmware functionality seem to be working. Need to test on real hardware with motors and test softserial next.

ino/takeup/takeup.ino

@@ -22,17 +22,20 @@
 #define Fbutton 3
 #define Bbutton 4
 
-const frameTime = 40; //ms
-const frameSpeed = 255;
+const int frameTime = 40; //ms
+const int frameSpeed = 255;
+const int buttonPress = 100; //ms
 
 /**
  * STATE
  **/
+boolean debug_state = false;
 
 const int Buttons[2] = {Fbutton, Bbutton};
-volatile int ButtonState[4] = {1, 1};
-volatile long ButtonTime[4] = {0, 0};
-volatile long buttontime = 0;
+volatile int ButtonState[2] = {1, 1};
+volatile long ButtonTime[2] = {0, 0};
+volatile int ButtonMultiple[2] = {0, 0};
+volatile long buttonTime = 0;
 
 const int Fmotor = 0;
 const int Bmotor = 1;
@@ -40,16 +43,23 @@ const int Bmotor = 1;
 volatile int Fspeed = 255;
 volatile int Bspeed = 255;
 
+
+volatile long Ftime = 0;
+volatile long Btime = 0;
 volatile boolean Frunning = false;
 volatile boolean Brunning = false;
 
-voilatile long timer = 0;
+volatile long timer = 0;
 
 /**
  * SERIAL
  **/
 
 volatile char cmdChar = 'z';
+const char cmd_debug = 'd';
+const char cmd_connect = 'i';
+const char cmd_mcopy_identifier = 'm';
+const char cmd_takeup_identifier = 'F';
 const char Fcmd = 'D';
 const char Bcmd = 'E';
 
@@ -76,32 +86,78 @@ void setup() {
 }
 
 void loop() {
+  timer = millis();
+  
   if (Serial.available()) {
     cmdChar = (char)Serial.read();
   }
-  if (cmdChar != 'z') {
-    cmd(cmdChar);
-  }
 
-  if (softSerial.available() > 0) {
-    cmdChar = (char)softSerial.read();
-  }
   if (cmdChar != 'z') {
     cmd(cmdChar);
   }
 
   cmdChar = 'z';
+
+  if (softSerial.available() > 0) {
+    cmdChar = (char)softSerial.read();
+  }
+
+  cmdChar = 'z';
+
+  btn(Fmotor);
+  btn(Bmotor);
+
+  if (Brunning || Frunning) {
+    monitor();
+  }
 }
 
 void cmd (char which) {
+  if (which == Fcmd && !Frunning) {
+    forward();
+  } else if (which == Bcmd && !Brunning) {
+    backward();
+  } else if (which == cmd_debug) {
+    debug();
+  } else if (which == cmd_connect) {
+    connect();
+  } else if (which == cmd_mcopy_identifier) {
+    identify();
+  }
+}
 
+void forward () {
+  log("forward()");
+  Ftime = timer;
+  set_speed(Fmotor, frameSpeed);
+  clockwise(Fmotor);
+}
+
+void backward () {
+  log("backward()");
+  Btime = timer;
+  set_speed(Bmotor, frameSpeed);
+  counter_clockwise(Bmotor);
+}
+
+void monitor () {
+  if (Frunning && timer - Ftime >= frameTime) {
+    stop(Fmotor);
+  }
+  if (Brunning && timer - Btime >= frameTime) {
+    stop(Bmotor);
+  }
+}
+
+void stop (int motor) {
+  brake_motor(motor);
 }
 
 //input value from 0 to 255
 void set_speed (int motor, int val){ 
-  if (motor == 0) {
+  if (motor == Fmotor) {
     Fspeed = val;
-  } else if (motor == 1) {
+  } else if (motor == Bmotor) {
     Bspeed = val;
   }
 }
@@ -188,14 +244,14 @@ void counter_clockwise (int motor){
 
 void brake (){
   if (Frunning) {
-    brakeMotor(Fmotor);
+    brake_motor(Fmotor);
   }
   if (Brunning) {
-    brakeMotor(Bmotor);
+    brake_motor(Bmotor);
   }
 }
 
-void brakeMotor (int motor) {
+void brake_motor (int motor) {
   if (motor == Fmotor) {
     digitalWrite(Fpos, LOW);
     digitalWrite(Fneg, LOW);
@@ -219,42 +275,47 @@ void btn (int index) {
   int val = digitalRead(Buttons[index]);
   if (val != ButtonState[index]) {
     if (val == LOW) { // pressed
-      ButtonTime[index] = millis();
-      button_start(index, buttontime);
-    } else if (val == HIGH) { // not pressed
-      buttontime = millis() - ButtonTime[index];
-      button_end(index, buttontime);
+      ButtonTime[index] = timer;
+      ButtonMultiple[index] = -1;
     }
   }
   if (val == LOW) {
-    
+    buttonTime = timer - ButtonTime[index];
+    btnCmd(index, buttonTime);
   }
   ButtonState[index] = val;
 }
 
-/* ------------------------------------------------
- *  Determines a specific action for each press length
- *  of each button.
- * ------------------------------------------------*/
-void button_end (int index, long buttontime) {
-  if (index == 0) { //forward
-    if (buttontime > 10) {
-      //
+void btnCmd (int index, int time) {
+  int multiple = floor(time / buttonPress);
+  if (multiple > ButtonMultiple[index]) {
+    if (index == Fmotor) {
+      cmd(Fcmd);
+    } else if (index == Bmotor) {
+      cmd(Bcmd);
     }
-  } else if (index == 1) { //backward
-    if (buttontime > 10) {
-      //
+    ButtonMultiple[index] = multiple;
   }
-  }
-  buttontime = 0;
 }
 
-/**
- * Vex = Higher voltage that can power the motor and/or the shield as well as the Arduino if wanted
- * 5V = Must be a clean 5V that can power the Arduino and shield, and if wanted also the motor itself
- * GND = as always, the ground of everything
- * B- = Connection of the first motor or first winding of the stepper motor
- * B+ = Connection of the first motor or first winding of the stepper motor
- * A- = Connection of the second motor or second winding of the stepper motor
- * A+ = Connection of the second motor or second winding of the stepper motor
- **/
+void connect () {
+  Serial.println(cmd_connect);
+  log("connect()");
+}
+
+void identify () {
+  Serial.println(cmd_takeup_identifier);
+  log("identify()");  
+}
+
+void debug () {
+  debug_state = true;
+  Serial.println(cmd_debug);
+  log("debugging enabled");
+}
+
+void log (String msg) {
+  if (debug_state) {
+    Serial.println(msg);
+  }
+}