Diff: Classes/RobotMVC/RobotModel.cpp

Revision:

15:e7fc3271bf53

Parent:

11:10a7bb4bc714

Child:

17:df86b0e2bb40

--- a/Classes/RobotMVC/RobotModel.cpp	Wed May 10 02:51:54 2017 +0000
+++ b/Classes/RobotMVC/RobotModel.cpp	Wed May 10 03:21:35 2017 +0000
@@ -36,8 +36,8 @@
     led_orange = 0;
     led_red = 0;
     led_blue = 0;
-    
-    //Line Detector Setup    
+
+    //Line Detector Setup
     // Chip must be deselected
     cs = 1;
     // Setup the spi for 8 bit data, high steady state clock,
@@ -55,7 +55,7 @@
 //    //Averaging
 //    spi.write(0x3C);
 //    cs = 1;
-    
+
     wait_ms(200);
     led_orange = 1;
     led_red = 1;
@@ -63,24 +63,27 @@
 }
 
 //read the indicated light sensor
-int RobotModel::checkLight(int x){
+int RobotModel::checkLight(int x)
+{
     return _cds[x];
 }
 
 //read bit b of the line array
-int RobotModel::checkLine(int b){
+int RobotModel::checkLine(int b)
+{
     return _larray[b];
 }
 
 //scan the ADC on channel n
-int RobotModel::scan(int n){
+int RobotModel::scan(int n)
+{
     // Select the device by seting chip select low
     cs = 0;
     spi.write(0x18);
     //Scan channel N
     //Command bits -- 1 CHS3 CHS2 CHS1     CHS0 1 1 0
     //CHSX is the binary for N
-    switch(n){
+    switch(n) {
         default:
         case 0:
             spi.write(0x84);
@@ -124,6 +127,47 @@
     return temp;
 }
 
+//Motor Control
+#include "motordriver.h"
+
+
+Motor A(D6, D5, D4, 1); // pwm, fwd, rev, can break
+Motor B(D3, D7, D2, 1); // pwm, fwd, rev, can break
+
+void StopMotors(void){
+    A.stop(1);
+    B.stop(1);
+    }
+
+void TurnRight(){
+    A.speed(1);
+    B.stop(1);
+    wait(.3);
+    }
+void TurnLeft(){
+    B.speed(1);
+    A.stop(1);
+    wait(.3);
+    }
+void DriveStraight(){
+    A.speed(1); //Right
+    B.speed(1); //Left
+    }
+void DriveSquare(){
+    DriveStraight();
+    wait(1);
+    TurnLeft();
+    DriveStraight();
+    wait(1);
+    TurnLeft();
+    DriveStraight();
+    wait(1);
+    TurnLeft();
+    DriveStraight();
+    wait(1);
+    TurnLeft();
+    }
+
 //update the model based on the mode
 int RobotModel::update()
 {
@@ -131,21 +175,37 @@
         led_green = 0;
     else
         led_green = !led_green;
-    
-    if(_mode == 2)
-    {
-        for(int i=0; i<8; i++)
-        {
-            scan(i);
-            _larray[i] = scan(i);
-        }
+        
+    switch(_mode) {
+        default:
+            //Reset
+        case 0:
+            StopMotors();
+            break;
+            //LF
+        case 2:
+            for(int i=0; i<8; i++) {
+                scan(i);
+                _larray[i] = scan(i);
+            }
+            break;
+            //OA
+        case 3:
+            break;
+            //LA
+        case 4:
+            scan(10);
+            _cds[0] = scan(10);
+            scan(8);
+            _cds[1] = scan(8);
+            break;
+            //TRC
+        case 5:
+            break;
+            //WiiC
+        case 6:
+            DriveSquare();
+            break;
     }
-    if(_mode == 4)
-    {
-         scan(10);
-         _cds[0] = scan(10);
-         scan(8);
-         _cds[1] = scan(8);
-    }
-    return 0;
+    return  (int)(ain.read()*100);
 }
\ No newline at end of file