Diff: main.cpp

Revision:

1:7b8696baf8ff

Parent:

0:966d81803039

Child:

2:2e7ed3a34dd1

--- a/main.cpp	Tue May 31 16:37:13 2016 +0000
+++ b/main.cpp	Thu Jan 18 22:34:56 2018 +0000
@@ -1,35 +1,64 @@
 #include "mbed.h"
 #include "SerialRPCInterface.h"
 #include "MPU6050.h"
+#include "TSL1401CL.h"
 #include "QEI.h"
- 
+void steerImprovedPointTurns(int8_t line_pos);
+void setLEDs(uint8_t a, uint8_t b, uint8_t c, uint8_t d);
 SerialRPCInterface SerialRPC(USBTX, USBRX, 115200);
  
-float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z;
-int r_enc, l_enc;
- 
+float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, left_pwm, right_pwm;
+int line_pos, line_pos_previous, line_lost_time;
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+PwmOut m1_fwd(p21);
+PwmOut m1_back(p22);
+PwmOut m2_fwd(p23);
+PwmOut m2_back(p24);
 RPCVariable<float> rpc_accel_x(&accel_x, "accel_x");
 RPCVariable<float> rpc_accel_y(&accel_y, "accel_y");
 RPCVariable<float> rpc_accel_z(&accel_z, "accel_z");
-RPCVariable<float> rpc_gryo_x(&gyro_x, "gyro_x");
-RPCVariable<float> rpc_gryo_y(&gyro_y, "gyro_y");
-RPCVariable<float> rpc_gryo_z(&gyro_z, "gyro_z");
-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);
+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<int> rpc_line_pos(&line_pos, "line_pos");
+
+//QEI l_wheel (p29, p30, NC, 624);
+//QEI r_wheel (p11, p12, NC, 624);
  
 MPU6050 mpu6050;
- 
-DigitalOut init_done(LED1);
-DigitalOut imu_good(LED2);
-DigitalOut main_loop(LED3);
+
+#define CAM_INTEGRATION_TIME 80
+
+// Higher line threshold -> the sensor will only recognize larger changes in
+// brightness as a line edge
+#define LINE_THRESHOLD 10
+#define LINE_PRECISION 2
+#define LINE_CROP_AMOUNT 4
+
+// These constants define the base pwm across the motors and how much the
+// controller
+// adjusts based on position of the line relative to the sensor
+#define SPEED_PWM 0.2
+#define TURN_SENS_INNER 1.5F
+#define TURN_SENS_OUTER 0.5F
+
+// Defines data
+#define LINE_HIST_SIZE 1000
+#define LINE_END_TIME 2500
+
+// Sensor pins
+#define clk p16
+#define si p17
+#define adc p18
  
 int main() {
-    init_done = 0;
-    imu_good = 0;
-    main_loop = 0;
-    
+    /********** SENSOR SETUP **********/
+    setLEDs(1, 1, 1, 1);
+        
     //Set up I2C
     i2c.frequency(400000);  // use fast (400 kHz) I2C
     
@@ -49,27 +78,20 @@
             mpu6050.getAres();
             mpu6050.getGres();
             imu_ready = true;
-            imu_good = 1;
         }
     }
     
-    init_done = 1;
-    uint8_t loop_count = 10;
+    setLEDs(0, 0, 0, 1);
+        
+    line_lost_time = 0;
     while(1) {
         wait_ms(10);
-        
-        // Handle the encoders
-        r_enc=r_wheel.getPulses();
-        l_enc=l_wheel.getPulses();
-        //pc.printf("Pulses are: %i, %i\r\n", l_enc,r_enc);
+        m1_fwd.write(left_pwm);
+        m2_fwd.write(right_pwm);
+        /***** Read line sensor *****/
         
-        if (!(--loop_count)) {
-            loop_count = 10;
-            main_loop = !main_loop;
-        }
-        
-        if (imu_ready) {
-            
+        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
@@ -86,4 +108,11 @@
             }
         }
     }
-}
\ No newline at end of file
+}
+
+void setLEDs(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
+  led1 = a;
+  led2 = b;
+  led3 = c;
+  led4 = d;
+}