Diff: DriveController.cpp

Revision:

4:ac6b2e5b240b

Parent:

3:0d7687b6ef14

Child:

5:f53f06a866e9

--- a/DriveController.cpp	Fri Oct 03 18:54:42 2014 +0000
+++ b/DriveController.cpp	Tue Oct 14 02:39:29 2014 +0000
@@ -2,41 +2,35 @@
 
 #define REFLECTION_THRESHOLD = 0
 
-/*DriveController::DriveController() : wheel1(PTC7), wheel2(PTC2), wheel3(PTE29), wheel4(PTE30), sensor01(PTC7),
-    sensor02(PTC0), sensor03(PTC3), sensor04(PTC4), sensor05(PTC5), sensor06(PTC6), sensor07(PTC10), sensor08(PTC11), 
-    sensor09(PTC9), sensor10(PTC8), sensor11(PTA5), sensor12(PTA4), sensor13(PTA12), sensor14(PTD4), sensor15(PTA2),
-    sensor16(PTA1), sensor17(PTC12), sensor18(PTC13), sensor19(PTC16), sensor20(PTC17), sensor21(PTD2), sensor22(PTD0),
-    sensor23(PTD5), sensor24(PTD13), orientation(NORTH)
+DriveController::DriveController() : orientation(NORTH), i2c(PTC2, PTC1), wheelSpeed1(PTB0), wheelSpeed2(PTB1), wheelSpeed3(PTB2),
+    wheelSpeed4(PTB3), wheelDirection1(PTE20), wheelDirection2(PTE21), wheelDirection3(PTE22), wheelDirection4(PTE23)
 {
-    command.direction = NORTH;
-    command.distance = 0;
-}*/
-
-DriveController::DriveController() : wheel1(PTC7), wheel2(PTC2), wheel3(PTE29), wheel4(PTE30), orientation(NORTH)
-{
+    
+    PinName sensorPins[] = {PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11, PTC9, PTC8, PTA5, PTA4, PTA12, PTD4, PTA2, PTA1,
+        PTC12, PTC13, PTC16, PTC17, PTD2, PTD0, PTD5, PTD13}; 
     command.direction = NORTH;
     command.distance = 0;
     
     for(int i = 0; i < 24; i++)
     {
-        sensors.push_back(new DigitalInOut(sensorPinNames[i]));
-    }
+        sensors.push_back(DigitalInOut(sensorPins[i]));
+    }    
 }
 
-DriveController::void go()
+void DriveController::go()
 {
     while(true)
     {
         getCommand();
         
-        if(currentCommand.distance != 0)
+        if(command.distance != 0)
             move();
             
         sendComplete();
     }    
 }
 
-DriveController::void move()
+void DriveController::move()
 {
     int travelled = 0;
     bool atIntersection = true;
@@ -60,35 +54,37 @@
     command.distance = 0;
 }
 
-DriveController::double calculateError()
+double DriveController::calculateError()
 {
-    
+    return 0;
 }
 
-DriveController::void forward(double correction)
+void DriveController::forward(double correction)
 {
     
 }
 
-DriveController::bool intersection()
+bool DriveController::intersection()
 {
-    return (sensorStates[0]||sensorStates[8]||sensorStates[16])&&(sensorStates[1]||sensorStates[9]||sensorStates[17])
-        &&(sensorStates[2]||sensorStates[10]||sensorStates[18])&&(sensorStates[4]||sensorStates[12]||sensorStates[20])
-        &&(sensorStates[5]||sensorStates[13]||sensorStates[21])&&(sensorStates[6]||sensorStates[14]||sensorStates[22])
-        &&(sensorStates[7]||sensorStates[15]||sensorStates[23]);
+    return sensorStates[8]&&sensorStates[9]&&sensorStates[10]&&sensorStates[12]&&sensorStates[13]&&sensorStates[14]&&sensorStates[15];
 }
 
-DriveController::void rotate(Direction diretion)
+void DriveController::rotate(Direction diretion)
 {
     
 }
 
-DriveController::void getCommand()
+void DriveController::getCommand()
 {
     
 }
 
-DriveControler::void readSensors()
+void DriveController::sendComplete()
+{
+    
+}
+
+void DriveController::readSensors()
 {
     
 }
\ No newline at end of file