Diff: main.cpp

Revision:

11:8fc2b703086b

Parent:

10:810d1849da9d

Child:

12:5790e56a056f

--- a/main.cpp	Sat May 06 22:49:31 2017 +0000
+++ b/main.cpp	Sun May 07 01:13:42 2017 +0000
@@ -9,19 +9,17 @@
 #include "stm32f4xx.h" 
 #include "QEI.h"
 
-// PID
-#define P_CONSTANT 0
-#define I_CONSTANT 0
-#define D_CONSTANT 0
 
+/* Constants for when HIGH_PWM_VOLTAGE = 0.2
 #define IP_CONSTANT 6
 #define II_CONSTANT 0
 #define ID_CONSTANT 1
+*/
 
-#include "QEI.h"
-#define PULSES 880
-#define SAMPLE_NUM 100
-
+// Constants for when HIGH_PWM_VOLTAGE = 0.1
+#define IP_CONSTANT 15
+#define II_CONSTANT 0
+#define ID_CONSTANT 0
 
 void moveForwardUntilWallIr() {    
     double currentError = 0;
@@ -29,15 +27,24 @@
     double derivError = 0;
     double sumError = 0;
     
-    double HIGH_PWM_VOLTAGE = 0.2;
+    double HIGH_PWM_VOLTAGE = 0.1;
     
-    double rightSpeed = 0.2;
-    double leftSpeed = 0.2;
+    double rightSpeed = 0.1;
+    double leftSpeed = 0.1;
     
     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
     float ir3 = IRP_3.getSamples( SAMPLE_NUM );
     
-    while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) {       
+    int count = encoder0.getPulses();
+    while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) {
+        int pulseCount = (encoder0.getPulses()-count) % 5600;
+        if (pulseCount > 5400 && pulseCount < 5800) {
+            blueLed.write(0);
+        } else {
+            blueLed.write(1);
+        }
+        
+        
         currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
         derivError = currentError - previousError;
         double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;       
@@ -78,29 +85,41 @@
     }
 }
 
+void printDipFlag() {
+    if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
+}
+
 void enableButton1() {
     dipFlags |= BUTTON1_FLAG;
+    printDipFlag();
 }
 void enableButton2() {
     dipFlags |= BUTTON2_FLAG;
+    printDipFlag();
 }
 void enableButton3() {
     dipFlags |= BUTTON3_FLAG;
+    printDipFlag();
 }
 void enableButton4() {
     dipFlags |= BUTTON4_FLAG;
+    printDipFlag();
 }
 void disableButton1() {
     dipFlags &= ~BUTTON1_FLAG;
+    printDipFlag();
 }
 void disableButton2() {
     dipFlags &= ~BUTTON2_FLAG;
+    printDipFlag();
 }
 void disableButton3() {
     dipFlags &= ~BUTTON3_FLAG;
+    printDipFlag();
 }
 void disableButton4() {
     dipFlags &= ~BUTTON4_FLAG;
+    printDipFlag();
 }
 
 int main()
@@ -119,7 +138,7 @@
 //    IR_4.write(1);
 
     redLed.write(1);
-    greenLed.write(0);
+    greenLed.write(1);
     blueLed.write(1);
     
     //left_motor.forward(0.1);
@@ -129,8 +148,8 @@
     // PA_0 is B of right
     // PA_5 is A of left
     // PB_3 is B of left
-    QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
-    QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
+    //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
+//    QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
     
     // TODO: Setting all the registers and what not for Quadrature Encoders
     RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
@@ -150,10 +169,11 @@
     
 
     while (1) {
+        
         moveForwardUntilWallIr();
         wait(0.1);
         //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
-        //serial.printf("Pulse Count=> e0:%d, e1:%d      \r\n", encoder0.getPulses(),encoder1.getPulses());
+//        serial.printf("Pulse Count=> e0:%d, e1:%d      \r\n", encoder0.getPulses(),encoder1.getPulses());
         //double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
         
         //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n",