Revision 0:d123694a690c, committed 2014-09-03

Comitter:

tomo21

Date:

Wed Sep 03 15:29:28 2014 +0000

Commit message:

Updated with initial code.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 03 15:29:28 2014 +0000
@@ -0,0 +1,381 @@
+#include "mbed.h"
+ 
+#define LIS3DH_CTRL_REG1 (0x20) // CTRL_REG1 Sample Rate
+#define LIS3DH_CTRL_REG4 (0x23) // CTRL_REG4 Resolution
+#define LIS3DH_CFG_REG  (0x1F)  // ADC, Temp Sensor Register
+#define LIS3DH_ADDR     (0x30)  // LIS3DH address
+ 
+I2C i2c(I2C_SDA, I2C_SCL);
+ 
+//DigitalOut myled(LED1);
+ 
+Serial pc(SERIAL_TX, SERIAL_RX);
+ 
+//volatile char Accel[] = "+abc.d C";
+
+uint8_t z_val_l, z_val_h, x_val_l, x_val_h, y_val_l, y_val_h, out_3_l, out_3_h;
+int16_t z_val, x_val, y_val; 
+int16_t start, finish, out_3, celsius, fahrenheit;
+
+float p, p1, p2;
+
+float threshold = -25;
+int accel_count = 20;
+int accel_counter = 20;
+float Q = .2; //mL per ms
+float volume_initial = 750;
+float volume_poured = 750;
+float volume_current = 750;
+
+float motor_current_limit = .04;
+
+enum motor_state {start_pour, valve_closed, valve_open, closing_time};
+
+motor_state state = valve_closed;
+
+DigitalOut motor1(PA_10);
+DigitalOut motor2(PB_3);
+
+//NEED TO DETERMINE PINS
+/*
+DigitalOut LED_1(PA_1);
+DigitalOut LED_2(PA_2);
+DigitalOut LED_3(PA_3);
+DigitalOut LED_4(PA_4);
+DigitalOut LED_5(PA_5);
+*/
+AnalogIn motor_current(PA_0);
+
+Timer volume_timer;
+
+void stop_motor(){
+    motor1 = 1;
+    motor2 = 1;
+    wait_us(10);
+    motor1 = 0;
+    motor2 = 0; 
+}
+
+void open_valve(){
+    motor1 = 1;
+    motor2 = 0;    
+}
+
+void close_valve(){
+    motor1 = 0;
+    motor2 = 1;    
+}
+
+
+void read_accel()
+{
+    
+        char data_write[2];
+        char data_read[2];
+ 
+        // Configure the Accelerometer device LIS3DH:
+
+        data_write[0] = LIS3DH_CTRL_REG1;
+        data_write[1] = 0x77;   //400hz, Normal mode, Z Y X axis on
+        i2c.write(LIS3DH_ADDR, data_write, 2, 0);
+    
+        data_write[0] = LIS3DH_CTRL_REG4;
+        data_write[1] = 0x08;   //2g, high res
+        i2c.write(LIS3DH_ADDR, data_write, 2, 0); 
+    
+        data_write[0] = LIS3DH_CFG_REG;
+        data_write[1] = 0xC0;   //ADC on, temp sensor on
+        i2c.write(LIS3DH_ADDR, data_write, 2, 0); 
+        
+        // Read Acceleration Registers
+        
+        //X axis
+        data_write[0] = 0x28; //X axis Low Byte
+        i2c.write(LIS3DH_ADDR, data_write, 1, 1); // no stop (unsure)
+        i2c.read(LIS3DH_ADDR, data_read, 1, 0);
+        x_val_l = data_read[0];
+        
+        data_write[0] = 0x29; //X axis High Byte
+        i2c.write(LIS3DH_ADDR, data_write, 1, 1); // no stop (unsure)
+        i2c.read(LIS3DH_ADDR, data_read, 1, 0);
+        x_val_h = data_read[0];
+        
+        //convert from two separate bytes to one int
+        x_val = ((x_val_h) << 8) | x_val_l;
+        
+        float x = ((float)x_val * 0.061035f) / 1000.0f;
+        
+        //Y axis
+        data_write[0] = 0x2A; //X axis Low Byte
+        i2c.write(LIS3DH_ADDR, data_write, 1, 1); // no stop (unsure)
+        i2c.read(LIS3DH_ADDR, data_read, 1, 0);
+        y_val_l = data_read[0];
+        
+        data_write[0] = 0x2B; //X axis High Byte
+        i2c.write(LIS3DH_ADDR, data_write, 1, 1); // no stop (unsure)
+        i2c.read(LIS3DH_ADDR, data_read, 1, 0);
+        y_val_h = data_read[0];
+        
+        //convert from two separate bytes to one int
+        y_val = ((y_val_h) << 8) | y_val_l;
+        
+        float y = ((float)y_val * 0.061035f) / 1000.0f;
+        
+        //Z axis
+        data_write[0] = 0x2C; //X axis Low Byte
+        i2c.write(LIS3DH_ADDR, data_write, 1, 1); // no stop (unsure)
+        i2c.read(LIS3DH_ADDR, data_read, 1, 0);
+        z_val_l = data_read[0];
+        
+        data_write[0] = 0x2D; //X axis High Byte
+        i2c.write(LIS3DH_ADDR, data_write, 1, 1); // no stop (unsure)
+        i2c.read(LIS3DH_ADDR, data_read, 1, 0);
+        z_val_h = data_read[0];
+        
+        //convert from two separate bytes to one int
+        z_val = ((z_val_h) << 8) | z_val_l;
+        
+        float z = ((float)z_val * 0.061035f) / 1000.0f;
+        
+        //Calculate Pitch (angle from X axis, which is vertical in the bottle orientation)
+        // pitch is in radians (-pi/2 to pi/2)
+                
+        float yz_sq = sqrt(y*y + z*z);
+        float pitch = atan2(x, yz_sq);
+        float pitch_deg = pitch * 57.2957795;
+             
+        //display result
+        
+        pc.printf("X acceleration = %f\n", x);
+        pc.printf("Y acceleration = %f\n", y);
+        pc.printf("Z acceleration = %f\n", z);
+        pc.printf("\n");
+        pc.printf("Pitch = %f\n", pitch_deg);
+        
+        //filter pitch values
+        p2 = p1;
+        p1 = p;
+        p += -.2 * (p - pitch_deg); 
+}
+
+
+int tilt_up()
+{
+        if (p2 < p1 && p1 < p)
+        {
+            accel_count = 20;
+            return 1;
+        }
+        else
+        { return 0;}
+}
+
+int detect_current_spike()
+{
+    if(motor_current.read() >= motor_current_limit)
+        {
+            //the motor has finished opening or closing the valve
+            return 1;
+        }
+    else
+        {
+            //the motor is still opening or closing the valve
+            return 0;
+        }
+}
+    
+
+
+
+
+int detect_tilt()
+{
+ //detect tilt algorithm
+        
+        //if the pitch goes below the threshold for 20 samples
+        if (p < threshold)
+        {
+            accel_count --;
+        }
+        //reset the sample count if the bottle is tilting upright
+        else if (p2 < p1 && p1 < p)
+        {
+            accel_count = accel_counter;
+        }
+        else 
+        {
+            accel_count = accel_counter;
+        }
+        
+        //
+        if (accel_count <=0)
+        {
+            //we're tilted!
+            return 1;
+        }
+        else
+        { return 0;}    
+        
+        
+}
+
+
+/*void LED_track()
+{
+    if(volume_current <= 150)
+    {
+     LED_1 = 0;
+     LED_2 = 0;
+     LED_3 = 0;
+     LED_4 = 0;
+     LED_5 = 0;
+    }
+    if(volume_current > 150 && volume_current <= 300)
+    {
+     LED_1 = 0;
+     LED_2 = 0;
+     LED_3 = 0;
+     LED_4 = 0;
+     LED_5 = 1;
+    }
+    if(volume_current > 300 && volume_current <= 450)
+    { 
+     LED_1 = 0;
+     LED_2 = 0;
+     LED_3 = 0;
+     LED_4 = 1;
+     LED_5 = 1;
+    }
+    if(volume_current > 450 && volume_current <= 600)
+    { 
+     LED_1 = 0;
+     LED_2 = 0;
+     LED_3 = 1;
+     LED_4 = 1;
+     LED_5 = 1;
+    }
+    if(volume_current > 600 && volume_current <= 750)
+    { 
+     LED_1 = 0;
+     LED_2 = 1;
+     LED_3 = 1;
+     LED_4 = 1;
+     LED_5 = 1;
+    }
+    if(volume_current > 750)
+    { 
+     LED_1 = 1;
+     LED_2 = 1;
+     LED_3 = 1;
+     LED_4 = 1;
+     LED_5 = 1;
+    }     
+     
+}
+*/ 
+int main()
+{
+ 
+    
+    
+    /*    
+    if (status != 0) { // Error
+        while (1) {
+            myled = !myled;
+            wait(0.2);
+        }
+    }
+    */   
+ 
+    while (1) {
+        
+        //pc.printf("start");
+        read_accel();
+        //pc.printf("here");
+        switch (state){
+            case valve_closed:
+            {
+                pc.printf("State = valve closed\n");
+                stop_motor();
+                volume_timer.stop();
+                float t_poured = volume_timer.read();
+                volume_poured = Q*t_poured;
+                volume_current -= volume_poured;
+                //LED_track();
+            
+                if (detect_tilt() == 1)
+                {
+                     state = start_pour;
+                     p1 = 0;
+                     p2 = 0;
+                     break;               
+                }
+                else {break;}
+            }
+            
+            case start_pour:
+            {
+                open_valve();
+                pc.printf("Opening Valve!\n");
+                //miss the inital current spike
+                wait(.1);
+                float cur = motor_current.read();
+                pc.printf("Current = %f\n", cur);
+                
+                if (tilt_up() == 1){
+                state = closing_time;    
+                }   
+                else if(detect_current_spike() == 1)
+                {
+                    state = valve_open;
+                    break;
+                }
+                else {break;}
+                
+            }
+            
+            case closing_time:
+            {
+                    close_valve();
+                    pc.printf("Closing Valve!\n");
+                    //miss the initial current spike
+                    wait(.05);
+                    float cur = motor_current.read();
+                    pc.printf("Current = %f\n", cur);
+                if(detect_current_spike() == 1)
+                {
+                    state = valve_closed;
+                    break;
+                }    
+                else {break;}
+            }
+            
+            case valve_open:
+            {
+                pc.printf("Case: valve_open\n");
+                stop_motor();
+                volume_timer.start();
+            
+                if (tilt_up() == 1)
+                {
+                     state = closing_time;
+                     break;               
+                }
+                else {break;}  
+            }   
+            
+        
+        
+        
+            
+        }
+        
+        
+            
+        
+        wait(.1);
+        
+    }
+
+}
+ 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Sep 03 15:29:28 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/6213f644d804
\ No newline at end of file