Diff: main.cpp

Revision:

2:2e7ed3a34dd1

Parent:

1:7b8696baf8ff

Child:

3:06820ff9a80c

--- a/main.cpp	Thu Jan 18 22:34:56 2018 +0000
+++ b/main.cpp	Mon May 21 18:50:38 2018 +0000
@@ -2,13 +2,25 @@
 #include "SerialRPCInterface.h"
 #include "MPU6050.h"
 #include "TSL1401CL.h"
-#include "QEI.h"
+#include "QEI_simple.h"
+#include <list>
+
+// Sensor pins
+#define clk p16
+#define si p17
+#define adc p18
+#define revs_per_count 894
+
 void steerImprovedPointTurns(int8_t line_pos);
 void setLEDs(uint8_t a, uint8_t b, uint8_t c, uint8_t d);
+void readCamera(Arguments* input, Reply* output);
+
 SerialRPCInterface SerialRPC(USBTX, USBRX, 115200);
  
-float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, left_pwm, right_pwm;
+float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, duty_cycle_left, duty_cycle_right;
 int line_pos, line_pos_previous, line_lost_time;
+//int r_enc, l_enc, l_phase, r_phase;
+TSL1401CL cam(clk, si, adc);
 
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
@@ -24,8 +36,12 @@
 RPCVariable<float> rpc_gyro_x(&gyro_x, "gyro_x");
 RPCVariable<float> rpc_gyro_y(&gyro_y, "gyro_y");
 RPCVariable<float> rpc_gyro_z(&gyro_z, "gyro_z");
+RPCVariable<float> rpc_left_duty_cycle(&duty_cycle_left, "duty_cycle_left");
+RPCVariable<float> rpc_right_duty_cycle(&duty_cycle_right, "duty_cycle_right");
 RPCVariable<int> rpc_line_pos(&line_pos, "line_pos");
-
+//RPCFunction rpc_camera_reading(&readCamera, "readCamera");
+//RPCVariable<int>   rpc_r_enc(&r_enc, "r_enc");
+//RPCVariable<int> rpc_l_enc(&l_enc, "l_enc");
 //QEI l_wheel (p29, p30, NC, 624);
 //QEI r_wheel (p11, p12, NC, 624);
  
@@ -58,7 +74,7 @@
 int main() {
     /********** SENSOR SETUP **********/
     setLEDs(1, 1, 1, 1);
-        
+
     //Set up I2C
     i2c.frequency(400000);  // use fast (400 kHz) I2C
     
@@ -80,18 +96,21 @@
             imu_ready = true;
         }
     }
+    //while(!cam.integrationReady());
+    //cam.read();
     
     setLEDs(0, 0, 0, 1);
-        
-    line_lost_time = 0;
+    duty_cycle_left = 0; duty_cycle_right = 0;
     while(1) {
+        /***** Read line sensor *****/
+        //cam.read();
         wait_ms(10);
-        m1_fwd.write(left_pwm);
-        m2_fwd.write(right_pwm);
-        /***** Read line sensor *****/
+        m1_fwd.write(duty_cycle_left);
+        m2_fwd.write(duty_cycle_right);
+        //r_enc=r_wheel.getPulses();
+        //l_enc=l_wheel.getPulses();
         
         if (imu_ready) {            
-            setLEDs(0, 1, 0, 0);
             if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
                 mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
                 mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
@@ -107,6 +126,17 @@
                 gyro_z = (float)gyroCount[2]*gRes - gyroBias[2];
             }
         }
+        /*
+        l_phase = l_enc % revs_per_count; r_phase = r_enc % revs_per_count;
+        if ((l_phase < revs_per_count / 2) && (r_phase < revs_per_count / 2)) {
+            setLEDs(1, 0, 1, 0);
+        } else if ((l_phase < revs_per_count / 2) && (r_phase > revs_per_count / 2)) {
+            setLEDs(1, 0, 0, 1);
+        } else if ((l_phase > revs_per_count / 2) && (r_phase < revs_per_count / 2)) {
+            setLEDs(0, 1, 1, 0);
+        } else {
+            setLEDs(0, 1, 0, 1);
+        }*/
     }
 }
 
@@ -116,3 +146,14 @@
   led3 = c;
   led4 = d;
 }
+/*
+void readCamera(Arguments* input, Reply* output){    
+    uint32_t pos = input->getArg<int>();       
+    int ans = 0;
+    setLEDs(1, 0, 0, 1);
+    //if (pos < 128u){
+    ans = cam.getData(pos);
+    //}   
+    output->putData(ans);
+    setLEDs(0, 1, 1, 0);
+}*/