Kuvée
This repository is for the Tilt to Pour algorithm using states and an interrupt driven loop.
Dependencies: mbed
Revision 0:381ace25a38e, committed 2014-09-04
- Comitter:
- tomo21
- Date:
- Thu Sep 04 20:19:48 2014 +0000
- Commit message:
- Tilt to pour working in interrupt loop.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Kuvee_defs.h Thu Sep 04 20:19:48 2014 +0000 @@ -0,0 +1,14 @@ +/* kuvee_defs.h + Primary include function for the Kuvee smart bottle + initally targeted to run on the ST Nucleo R030R8 board, eventually to be ported over to our own board + + + This header file includes critical defines that will be used througout the system + */ + + #ifndef _KUVEE_DEFS_H + #define _KUVEE_DEFS_H + + #define MAIN_LOOP_RATE_uS 10000 //The main loop runs every this many microseconds (allowable range 0-65535) + #define MAIN_LOOP_FREQ_Hz (1000000.0/ ((float) MAIN_LOOP_RATE_uS)) //The main loop runs every this many microseconds + #endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Sep 04 20:19:48 2014 +0000
@@ -0,0 +1,461 @@
+/* main.cpp
+ Primary function for the Kuvee smart bottle
+ initally targeted to run on the ST Nucleo R030R8 board, eventually to be ported over to our own board
+ the code initally controls the opening and closing of the valve
+ It will be extended to manage all of the peripherals in the system, and to communicate the data to the Android processor
+
+ The primary loop is driven by a Ticker, which sets a flag indicating to the main loop that the code should run
+ it is critical that this tick_int function remains simple. Also, any code written in the main loop must not block
+*/
+
+#include "mbed.h"
+#include "Kuvee_defs.h"
+//debug
+//#include <string.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
+
+/* declare pins */
+I2C i2c(I2C_SDA, I2C_SCL);
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+
+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;
+
+//float p, p1, p2, p3, p4, p5, p6, p7, p8, p9, p10;
+float p, p1, p2, p3, p4;
+
+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 = .03;
+float cur = 0;
+float pitch_deg;
+
+enum motor_state {start_pour, valve_closed, valve_open, closing_time};
+
+motor_state state = valve_closed;
+motor_state previous_state = valve_closed;
+
+DigitalOut motor1(PA_10);
+DigitalOut motor2(PB_3);
+
+//NEED TO DETERMINE PINS for LEDs
+/*
+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_1);
+
+Timer volume_timer;
+Timer pour_timer;
+Timer blanking_timer;
+
+Ticker heartbeat;
+DigitalOut led1(LED1);
+/* End declare pins */
+
+
+/* Set up Interrupt */
+static bool tick = false; // this is the primary flag that indicates that it is time to run the control loop
+static int ERROR_Overrun = 0; //this counts the number of times that the code was still running when the interrupt tried to trigger it again (ie code is running too slow)
+
+void tick_int() {
+ if (!tick) tick = true; //detect if interrupt has not yet finished
+ else ERROR_Overrun++ ;
+}
+/* End Set up Interrupt */
+
+
+/* Function Definitions */
+void stop_motor(){
+ 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);
+ 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
+ //p10 = p9;
+ //p9 = p8;
+ //p8 = p7;
+ //p7 = p6;
+ //p6 = p5
+ //p5 = p4;
+ p3 = p2;
+ p2 = p1;
+ p1 = p;
+ p += -.5 * (p - pitch_deg);
+}
+
+
+int tilt_up()
+{
+ //if (p10 < p9 && p9 < p8 && p8 < p7 && p7 < p6 && p6 < p5 && p5 < p4 && p4 < p3 && p3 < p2 && p2 < p1 && p1 < p)
+ if (p3 < p2 && p2 < p1 && p1 < p && (p-p3) >= 10)
+ {
+ accel_count = accel_counter;
+ return 1;
+ }
+ else
+ { return 0;}
+}
+
+int detect_current_spike()
+{
+ if(cur >= 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;
+ }
+
+}
+*/
+
+/* End Function Definitions */
+
+
+
+int main() {
+ //pc.baud(57600);
+ heartbeat.attach_us(&tick_int, MAIN_LOOP_RATE_uS); // the address of the function to be attached (tick_int) and the interval 10000uS
+ // spin in a main loop. hearbeat will interrupt it to call tick_int
+ while(1) {
+
+ while(tick){ //put real-time code in here
+
+ //pc.printf("start");
+ read_accel();
+ cur = motor_current.read();
+ //pc.printf("here");
+ previous_state = state;
+ 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;
+ p3 = 0;
+ p4 = 0;
+ //p5 = 0;
+ //p6 = 0;
+ //p7 = 0;
+ //p8 = 0;
+ //p9 = 0;
+ //p10 = 0;
+ //drive valve open for a set time
+ pour_timer.start();
+ break;
+ } else {
+ break;
+ }
+ }
+
+ case start_pour: {
+ open_valve();
+// //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;
+ blanking_timer.start();
+ break;
+ }
+// else if(detect_current_spike() == 1) {
+// state = valve_open;
+// break;
+// }
+ else if(pour_timer.read() >= .2){
+ state = valve_open;
+ pour_timer.stop();
+ pour_timer.reset();
+ break;
+ }
+ else {
+ break;
+ }
+
+ }
+
+ case closing_time: {
+ close_valve();
+ //miss the initial current spike
+ //wait(.05);
+ if (blanking_timer.read() >= .1){
+ blanking_timer.stop();
+ //pc.printf("Current = %f\n", cur);
+ if(detect_current_spike() == 1) {
+ pc.printf("Current Spike = %f\n", cur);
+ //pc.printf("CURRENT SPIKE\n");
+ state = valve_closed;
+ blanking_timer.reset();
+ break;
+ } else {
+ break;
+ }
+ }
+ }
+
+ case valve_open: {
+ stop_motor();
+ volume_timer.start();
+
+ if (tilt_up() == 1) {
+ state = closing_time;
+ blanking_timer.start();
+ break;
+ } else {
+ break;
+ }
+ }
+
+
+
+
+
+ }
+
+
+ tick = false;
+ }
+ //put any non-real time code below here - make sure that it never blocks long enough to prevent the real time code from running
+ //pc.printf("State = %s\n", state_debug);
+ pc.printf("Current = %f\n", cur);
+ if (state == closing_time){
+
+ pc.printf("P = %f\n", p);
+ pc.printf("P1 = %f\n", p1);
+ pc.printf("P2 = %f\n", p2);
+ pc.printf("P3 = %f\n", p3);
+
+ }
+ if (state != previous_state){
+ pc.printf("\nPitch = %f\n", pitch_deg);
+ if (state == valve_closed){
+ pc.printf("State = valve_closed\n");
+ }
+ if (state == start_pour){
+ pc.printf("State = start_pour\n");
+ }
+ if (state == closing_time){
+ pc.printf("State = closing_time\n");
+ }
+ if (state == valve_open){
+ pc.printf("State = valve_open\n");
+ }
+ }
+
+
+
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Sep 04 20:19:48 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/6213f644d804 \ No newline at end of file