Revision 18:eb675df59c7f, committed 2014-06-30

Comitter:

backman

Date:

Mon Jun 30 07:01:58 2014 +0000

Parent:

17:3dac99cf2b89

Child:

19:4869b10a962e

Commit message:

task_de

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Stack.cpp	Mon Jun 30 07:01:58 2014 +0000
@@ -0,0 +1,75 @@
+/*
+ * mbed library for Stack
+ * Copyright (c) 2011 Hiroshi Suga
+ * Released under the MIT License: http://mbed.org/license/mit
+ */
+
+/** @file Stack.cpp
+ * @brief Stack
+ */
+
+#include "Stack.h"
+
+template <class T>
+Stack<T>::Stack (int p_size) {
+    size = p_size + 1;
+    buf = new T[size];
+//    buf = (T*)malloc(sizeof(T) * size);
+    addr = 0;
+}
+
+template <class T>
+Stack<T>::~Stack () {
+    delete [] buf;
+//    free(buf);
+}
+
+template <class T>
+int Stack<T>::push (T dat) {
+
+    if (addr >= size) {
+        return -1;
+    }
+    buf[addr] = dat;
+    addr ++;
+    return dat;
+}
+
+template <class T>
+int Stack<T>::pop (T *dat) {
+
+    if (addr == 0) {
+        return -1;
+    }
+    addr --;
+    *dat = buf[addr];
+    return 0;
+}
+
+template <class T>
+int Stack<T>::read (T *dat) {
+
+    if (addr == 0) {
+        return -1;
+    }
+    *dat = buf[addr];
+    return 0;
+}
+
+template <class T>
+int Stack<T>::available () {
+    return size - addr;
+}
+
+template <class T>
+int Stack<T>::use () {
+    return addr;
+}
+
+template <class T>
+void Stack<T>::clear () {
+    addr = 0;
+}
+
+template class Stack<int>;
+template class Stack<float>;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Stack.h	Mon Jun 30 07:01:58 2014 +0000
@@ -0,0 +1,51 @@
+/*
+ * mbed library for Stack
+ * Copyright (c) 2011 Hiroshi Suga
+ * Released under the MIT License: http://mbed.org/license/mit
+ */
+
+/** @file Stack.h
+ * @brief Stack
+ */
+
+#ifndef Stack_H
+#define Stack_H
+
+#include "mbed.h"
+
+/** Stack class
+ */
+template <class T>
+class Stack {
+public:
+    /** init Stack class
+     * @param p_size size of stack
+     */
+    Stack (int p_size);
+    ~Stack ();
+
+    /** push to stack
+     * @param dat data
+     * @return data or -1:error
+     */
+    int push (T dat);
+
+    /** pop from stack
+     * @param dat data
+     * @return 0:ok / -1:error
+     */
+    int pop (T *dat);
+
+    int read (T *dat);
+
+    void clear ();
+    int available ();
+    int use ();
+
+private:
+    T *buf;
+    int size;
+    int addr;
+};
+
+#endif

--- a/main.cpp	Mon Jun 30 03:37:05 2014 +0000
+++ b/main.cpp	Mon Jun 30 07:01:58 2014 +0000
@@ -6,6 +6,7 @@
 #include "motor_api.h"
 #include "pot.h"
 #include "TSISensor.h"
+#include "Stack.h"
 
 #define Debug_cam_uart
 #define L_eye
@@ -15,8 +16,8 @@
 #define task_ma_time
 
 
-#define R_target 20
-#define L_target 108
+#define R_target 64
+#define L_target 64
 
 
 
@@ -42,7 +43,11 @@
                                  // 90/30=3
 PID cam_to_M_ctrlr(10.0,118.0,0.046,0.083,0.083-0.046,0.00,0.00,10);
 
-DigitalOut task_pin(PTD1);
+#ifdef task_ma_time
+DigitalOut cam_p(PTD1); //cam       black
+DigitalOut servo_p(PTA13); //servo   coffee
+DigitalOut de_p(PTD3);   //   red
+#endif
 TSISensor tsi;
 
 //os
@@ -50,6 +55,12 @@
 
 
 
+//global resource
+
+Stack<int> points(10);
+
+
+
 
 
 
@@ -67,16 +78,23 @@
 void cam_thread(void const *args){
     
     while(true){
-     
+        #ifdef task_ma_time
+        cam_p=1;
+        #endif   
            cam.read();
         
      
             b_r_c=cam.black_centerR();
-            b_l_c=cam.black_centerL();
-            //if(b_r_c==-1)
-             //  b_r_c=pre_b_r_c;
-            
-            //pre_b_r_c=b_r_c;
+           // b_l_c=cam.black_centerL();
+           
+           points.push(b_r_c);           
+           
+           
+        #ifdef task_ma_time
+        cam_p=0;
+        #endif    
+           
+           
             Thread::wait(t_cam);
         
         }
@@ -87,7 +105,10 @@
     
     while(1){
         
-        
+          
+        #ifdef task_ma_time
+        de_p=0;
+        #endif       
         
         
     stdio_mutex.lock();    
@@ -127,7 +148,10 @@
          stdio_mutex.unlock();
      
          
-     
+           
+        #ifdef task_ma_time
+        de_p=1;
+        #endif   
        
          
          
@@ -146,18 +170,33 @@
     
 
         while(1){
+     
+        #ifdef task_ma_time
+        servo_p=1;
+        #endif
         
-        if(b_r_c!=-1)
+        int point;
+        
+        for(;points.available()!=0;){
+            points.pop(&point);
+        
+        //algorithm
+        
+        }
+             
+        //if(b_r_c!=-1)
          v_servo=cam_to_M_ctrlr.compute(b_r_c,R_target);     
-        if(b_l_c!=-1)
-         v_servo=cam_to_M_ctrlr.compute(b_l_c,L_target);     
+        //if(b_l_c!=-1)
+        // v_servo=cam_to_M_ctrlr.compute(b_l_c,L_target);     
             
-         
-         
-            
-
+        
              // v_servo=pot2.read();
-               servo.set_angle(v_servo);
+         servo.set_angle(v_servo);
+        
+        #ifdef task_ma_time
+        servo_p=0;
+        #endif 
+        
               
          Thread::wait(20);    
         }    
@@ -216,8 +255,8 @@
       Thread th_c(cam_thread);
    //   Thread thread(ctrl_thread);  
       Thread th_s(servo_thread);  
-      Thread th_m(motor_thread);     
-   //  Thread th_de(de_thread);  
+   //   Thread th_m(motor_thread);     
+    // Thread th_de(de_thread);  
      while(1){