Revision 1:82bc25a7b68b, committed 2014-06-03

Comitter:

backman

Date:

Tue Jun 03 15:53:55 2014 +0000

Parent:

0:68c173249c01

Child:

2:c51647d3c14d

Commit message:

cam_algroithm

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bx-adc.cpp	Tue Jun 03 15:53:55 2014 +0000
@@ -0,0 +1,94 @@
+#include "bx-adc.h"
+#include "clk_freqs.h"
+ 
+#define MAX_FADC            6000000
+#define CHANNELS_A_SHIFT    5
+ 
+FastAnalogIn::FastAnalogIn(PinName pin, bool enabled)
+{
+    ADCnumber = (ADCName)pinmap_peripheral(pin, PinMap_ADC);
+    if (ADCnumber == (ADCName)NC) {
+        error("ADC pin mapping failed");
+    }
+ 
+    SIM->SCGC6 |= SIM_SCGC6_ADC0_MASK;
+ //-----------------------------------------------------
+    uint32_t port = (uint32_t)pin >> PORT_SHIFT;
+    SIM->SCGC5 |= 1 << (SIM_SCGC5_PORTA_SHIFT + port);
+ 
+    uint32_t cfg2_muxsel = ADC_CFG2_MUXSEL_MASK;
+    if (ADCnumber & (1 << CHANNELS_A_SHIFT)) {
+        cfg2_muxsel = 0;
+    }
+ //------------------------------------------------------    
+    // bus clk
+    uint32_t PCLK = bus_frequency();
+    uint32_t clkdiv;
+    for (clkdiv = 0; clkdiv < 4; clkdiv++) {
+        if ((PCLK >> clkdiv) <= MAX_FADC)
+            break;
+    }
+    if (clkdiv == 4)                    //Set max div
+        clkdiv = 0x7;
+ 
+    ADC0->SC1[1] = ADC_SC1_ADCH(ADCnumber & ~(1 << CHANNELS_A_SHIFT));
+ 
+    ADC0->CFG1 = ADC_CFG1_ADIV(clkdiv & 0x3)    // Clock Divide Select: (Input Clock)/8
+               | ADC_CFG1_MODE(3)               // (16)bits Resolution
+               | ADC_CFG1_ADICLK(clkdiv >> 2);  // Input Clock: (Bus Clock)/2
+ 
+    ADC0->CFG2 = cfg2_muxsel            // ADxxb or ADxxa channels
+               | ADC_CFG2_ADACKEN_MASK  // Asynchronous Clock Output Enable
+               | ADC_CFG2_ADHSC_MASK;   // High-Speed Configuration
+ 
+    ADC0->SC2 = ADC_SC2_REFSEL(0);      // Default Voltage Reference
+ 
+    pinmap_pinout(pin, PinMap_ADC);
+ 
+    //Enable channel
+    running = false;
+    enable(enabled);
+}
+ 
+void FastAnalogIn::enable(bool enabled)
+{
+    //If currently not running
+    if (!running) {
+        if (enabled) {
+            //Enable the ADC channel
+            ADC0->SC3 |= ADC_SC3_ADCO_MASK;       // Enable continuous conversion
+            ADC0->SC1[0] = ADC_SC1_ADCH(ADCnumber & ~(1 << CHANNELS_A_SHIFT));  //Start conversion
+            running = true;
+        } else
+            disable();
+    }
+}
+ 
+void FastAnalogIn::disable( void )
+{
+    //If currently running
+    if (running) {
+        ADC0->SC3 &= ~ADC_SC3_ADCO_MASK;      // Disable continuous conversion
+    }
+    running = false;
+}
+ 
+uint16_t FastAnalogIn::read_u16()
+{
+    if (!running)
+    {
+        // start conversion
+        ADC0->SC1[0] = ADC_SC1_ADCH(ADCnumber & ~(1 << CHANNELS_A_SHIFT));
+        // Wait Conversion Complete
+        while ((ADC0->SC1[0] & ADC_SC1_COCO_MASK) != ADC_SC1_COCO_MASK);
+    }
+    if(running && ((ADC0->SC1[0]&ADC_SC1_ADCH_MASK) != (ADC_SC1_ADCH(ADCnumber & ~(1 << CHANNELS_A_SHIFT)))))
+    {
+        running = false;
+        enable();
+        while ((ADC0->SC1[0] & ADC_SC1_COCO_MASK) != ADC_SC1_COCO_MASK);
+    }
+    // Return value
+    return (uint16_t)ADC0->R[0];
+}
+ 
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bx-adc.h	Tue Jun 03 15:53:55 2014 +0000
@@ -0,0 +1,117 @@
+#include "mbed.h"
+#include "pinmap.h"
+ 
+
+ 
+ /** A class similar to AnalogIn, only faster, for LPC1768, LPC408X and KLxx
+ *
+ * AnalogIn does a single conversion when you read a value (actually several conversions and it takes the median of that).
+ * This library runns the ADC conversion automatically in the background.
+ * When read is called, it immediatly returns the last sampled value.
+ *
+ * LPC1768 / LPC4088
+ * Using more ADC pins in continuous mode will decrease the conversion rate (LPC1768:200kHz/LPC4088:400kHz).
+ * If you need to sample one pin very fast and sometimes also need to do AD conversions on another pin,
+ * you can disable the continuous conversion on that ADC channel and still read its value.
+ *
+ * KLXX
+ * Multiple Fast instances can be declared of which only ONE can be continuous (all others must be non-continuous).
+ *
+ * When continuous conversion is disabled, a read will block until the conversion is complete
+ * (much like the regular AnalogIn library does).
+ * Each ADC channel can be enabled/disabled separately.
+ *
+ * IMPORTANT : It does not play nicely with regular AnalogIn objects, so either use this library or AnalogIn, not both at the same time!!
+ *
+ * Example for the KLxx processors:
+ * @code
+ * // Print messages when the AnalogIn is greater than 50%
+ *
+ * #include "mbed.h"
+ *
+ * FastAnalogIn temperature(PTC2); //Fast continuous sampling on PTC2
+ * FastAnalogIn speed(PTB3, 0);    //Fast non-continuous sampling on PTB3
+ *
+ * int main() {
+ *     while(1) {
+ *         if(temperature > 0.5) {
+ *             printf("Too hot! (%f) at speed %f", temperature.read(), speed.read());
+ *         }
+ *     }
+ * }
+ * @endcode
+ * Example for the LPC1768 processor:
+ * @code
+ * // Print messages when the AnalogIn is greater than 50%
+ *
+ * #include "mbed.h"
+ *
+ * FastAnalogIn temperature(p20);
+ *
+ * int main() {
+ *     while(1) {
+ *         if(temperature > 0.5) {
+ *             printf("Too hot! (%f)", temperature.read());
+ *         }
+ *     }
+ * }
+ * @endcode
+*/
+class FastAnalogIn {
+ 
+public:
+     /** Create a FastAnalogIn, connected to the specified pin
+     *
+     * @param pin AnalogIn pin to connect to
+     * @param enabled Enable the ADC channel (default = true)
+     */
+    FastAnalogIn( PinName pin, bool enabled = true );
+    
+    ~FastAnalogIn( void )
+    {
+        disable();
+    }
+    
+    /** Enable the ADC channel
+    *
+    * @param enabled Bool that is true for enable, false is equivalent to calling disable
+    */
+    void enable(bool enabled = true);
+    
+    /** Disable the ADC channel
+    *
+    * Disabling unused channels speeds up conversion in used channels. 
+    * When disabled you can still call read, that will do a single conversion (actually two since the first one always returns 0 for unknown reason).
+    * Then the function blocks until the value is read. This is handy when you sometimes needs a single conversion besides the automatic conversion
+    */
+    void disable( void );
+    
+    /** Returns the raw value
+    *
+    * @param return Unsigned integer with converted value
+    */
+    unsigned short read_u16( void );
+    
+    /** Returns the scaled value
+    *
+    * @param return Float with scaled converted value to 0.0-1.0
+    */
+    float read( void )
+    {
+        unsigned short value = read_u16();
+        return (float)value * (1.0f/65535.0f);
+    }
+    
+    /** An operator shorthand for read()
+    */
+    operator float() {
+        return read();
+    }
+ 
+    
+private:
+    bool running;    
+    char ADCnumber;
+    uint32_t *datareg;
+};
+ 

--- a/camera_api.cpp	Sun May 25 12:41:59 2014 +0000
+++ b/camera_api.cpp	Tue Jun 03 15:53:55 2014 +0000
@@ -0,0 +1,91 @@
+#include "mbed.h"
+#include "camera_api.h"
+
+#define clk 2  //ms
+
+ 
+ BX_camera::BX_camera(char cam){
+     
+     
+     cam_clk= new DigitalOut(PTE1) ;
+
+     si = new DigitalOut(PTD7);
+    
+     line_Cam = new FastAnalogIn(PTD5); 
+     
+     
+     
+     
+     
+     
+     }
+ 
+ 
+void BX_camera::read(void){
+ 
+ 
+        w_f_v=0x0000;
+        b_f_v=0xffff;
+ 
+ 
+        
+           *si=1;
+            *cam_clk=1;
+           
+           wait_us(30);   // tune here
+           *si=0;
+           *cam_clk=0;
+            
+            
+ 
+          line_Cam->enable();
+          
+          
+       //input 128   
+          
+          for(int i=0;i<128;i++){     
+             *cam_clk=1;   
+             wait_us(5);
+             
+             
+             line_image[i]=line_Cam->read_u16();
+         
+         
+         //  big small    
+             if(line_image[i] > w_f_v) 
+                w_f_v=line_image[i];
+             else if(line_image[i] < b_f_v )
+                b_f_v = line_image[i];
+                
+             
+             
+             
+             *cam_clk=0;
+             wait_us(5);
+        
+           
+           }
+           
+           
+           line_Cam->disable();
+           
+           
+           
+           //filter
+           
+          for(int i=0;i<128;i++){   
+           
+           
+               if( (line_image[i]-b_f_v) < (w_f_v - line_image[i] )    ) 
+                   sign_line_image[i]=' ';
+               else
+                   sign_line_image[i]='O';    
+           
+              if(i==0)
+                 sign_line_image[i]='X';
+           
+          }
+           
+           
+           
+           }
\ No newline at end of file

--- a/camera_api.h	Sun May 25 12:41:59 2014 +0000
+++ b/camera_api.h	Tue Jun 03 15:53:55 2014 +0000
@@ -1,4 +1,4 @@
-
+#include "bx-adc.h"
 
 
 class BX_camera{
@@ -7,17 +7,27 @@
     
     public:
     
+    BX_camera(char cam);
     
+    void read(void); //block in here
+    
+    
+    unsigned int line_image[128];
+    
+    char sign_line_image[128];
     
     private:
     
     
-    DigitalOut *line_CLK;
+    DigitalOut *cam_clk;
 
     DigitalOut *si;
-
+    
+    FastAnalogIn *line_Cam; 
 
+    int w_f_v;
     
+    int b_f_v;
     
     
     

--- a/main.cpp	Sun May 25 12:41:59 2014 +0000
+++ b/main.cpp	Tue Jun 03 15:53:55 2014 +0000
@@ -1,25 +1,311 @@
 #include "mbed.h"
-
-BX_servo servo;
+#include "servo_api.h"
+#include "camera_api.h"
+//BX_servo servo;
 
 
-
-
+Serial pc(USBTX, USBRX);
+BX_servo servo; 
+ 
+    BX_camera cam('n');
+    PwmOut cam_clk(PTC3);
 
 int main() {
     
+  /*  
+    int black_va;
+    int white_va;
+    */
+    
+    
+     pc.baud(115200);
+     
+     
+     
+     
+     char find_type=0x00;
+     
+     int b_end;
+     int b_start;
+     
+     int b2_end;
+     int b2_start;
+     
+     
+     int center;
+     int j=64;
+     bool f1;
+     bool f2;
+     bool f3;
+     bool f4;
+     while(1){
+         
+         
+         cam.read();
+         
+        for(int i=0;i<128;i++){
+             if(i==64)
+               pc.printf("X");
+             else          
+               pc.printf("%c", cam.sign_line_image[i]);
+         }
+         pc.printf("\r\n");
+         
+       
+       
+       
+         // find center 
+         // case 1      //  |  //
+         //case 2         / |  /
+         
+            if(cam.sign_line_image[64]==' ')
+              find_type=0x02;
+            else
+              find_type=0x01;  
+            
+            b_start=0;
+            b_end=0;   
+            b2_start=0;
+            b2_end=0;
+            
+            j=64;
+            f1=false;
+            f2=false;
+            f3=false;
+            f4=false;
+           for(int i=64;i<128; i++,j--){
+               
+                
+               switch(find_type){
+                   
+                   
+                   case 0x01:
+                       
+                         if(f1==false&&cam.sign_line_image[i]==' '){ 
+                                  if(f1==false){
+                                      b_start=i;
+                                      f1=true;
+                                  }
+                                  
+                         }
+                         if(f1== true&& f2==false&&cam.sign_line_image[i]=='O'){ 
+                                  if(f2==false){
+                                      b_end=i-1;
+                                      f2=true;
+                                  }
+                                  
+                         }
+                         
+                         if(f3==false&&cam.sign_line_image[j]==' '){ 
+                                  if(f3==false){
+                                      b2_end=j;
+                                      f3=true;
+                                  }
+                                  
+                         }
+                         if(f3==true&&f4==false&&cam.sign_line_image[j]=='O'){ 
+                                  if(f4==false){
+                                      b2_start=j-1;
+                                      f4=true;
+                                  }
+                                  
+                         }
+                         
+                         
+                         
+                       
+                       
+                       
+                   
+                   
+                   break;
+                   
+                   
+                   case 0x02:
+                   
+                         if(cam.sign_line_image[i]=='O'){         
+                    
+                                  if(f1==false){
+                                      b_end=i;
+                                      f1=true;
+                                  }
+                         }  
+                    
+                         if(cam.sign_line_image[j]=='O'){
+                        
+                                   if(f2==false){
+                                       b_start=j;     
+                                      f2=true;
+                                  }
+       
+                        }
+                   
+                   
+                   break;    
+                   
+                   
+                   
+                   
+               }
+        
+               
+           }
+       
+            
+          switch(find_type){
+         
+             case 0x01:
+                
+               if((b_end-b_start)>(b2_end-b2_start)    )
+                     center=(b_end+b_start)/2;
+                     
+               else  
+                     center=(b2_end+b2_start)/2;
+              
+                pc.printf("%d %d | %d  %d\r\n",b_start,b_end,b2_start,b2_end);
+             
+             
+             break;
+             
+             case 0x02:
+               center=(b_end+b_start)/2;
+              
+             break;
+         }
+           pc.printf("center :%d\r\n",center);
+   //--------------------------------------------            
+               
+       
+       
+         
+         }
+     
+     
+     
+       
+     
+     
+     
+     
+     
+     
+     
+     
+     
+     
+     
+     
+     
+     //tmp
+     int angle;
+     float black_pR=0;
+     float black_pL=0;
+    float black_pT=0;
+     //tune
+/*      
+     int min_va=90000;
+     int max_va=0;
+     int mid_va=0;
+  */
+  
+  
+    
+    while(1){
+         black_pR=0;
+         black_pL=0;
+         black_pT=0;
+  //       min_va=90000;
+    //     max_va=0;
+         angle=0;
+         cam.read();
+          for(int i=0;i<64;i++){
+                 
+                 
+                // angle+=cam.line_image[i]-cam.line_image[127-i];
+                 if(cam.line_image[i]==0)
+                     black_pR++;
+                 if(cam.line_image[127-i]==0)
+                    black_pL++;      
+             }
+             
+             
+             black_pT=black_pL+black_pR;
+                 
+               
+               if(black_pL>black_pT/2){
+               
+                 angle=(black_pR/black_pT)*90;
+                
+                  //angle=
+               }
+               else if(black_pR>black_pT/2){
+                     
+                     angle=-1.0*(black_pR/black_pT)*90;
+                      //angle=-0.5;
+               }
+               else{
+                   
+                 angle=0;
+                   
+               }
+                servo.set_angle(angle);
+                pc.printf("ang: %d",angle);
+             pc.printf("\r\n");
+               
+               
+               
+               
+                     
+                 
+                 
+            }
+             
+             
+             
+             
+             
+            
+             
+            // pc.printf("mid: %d|min: %d | max: %d | diff: %d",mid_va,min_va,max_va,max_va-min_va);
+             
+            
+             
+             
+             
+           //p - controller
+             
+           //servo angle not yet contorl  
+             
+             
+             
+             
+             
+             
+             
+             
+             
+             
+             
+             
+             
+             
+             
+             
+             
+  
+    
+    
+    
+  
+        
+        
+          
+          
+    
     
     
     
-    
-    
-    
-    
-    
-    
-    
-    
-    
+    return 0;
     
     
 }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor_api.h	Tue Jun 03 15:53:55 2014 +0000
@@ -0,0 +1,33 @@
+#include "mbed.h"
+
+
+class BX_motor{
+    
+    public:
+    
+       BX_motor(char type);
+       
+    
+    
+       void rotate(float level);
+       
+       
+       
+    private:
+    
+      float Level;
+       
+      char Type;
+      
+      PwmOut* forward_A;
+      PwmOut* backward_A;
+
+      PwmOut* forward_B;
+      PwmOut* backward_B;
+
+
+      DigitalOut* engine_enable;
+
+    
+    
+    };
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servo_api.cpp	Tue Jun 03 15:53:55 2014 +0000
@@ -0,0 +1,50 @@
+// 0~180 angle    1~2ms
+#include "mbed.h"
+#include "servo_api.h"
+
+
+#define right_end 0.05  //90
+
+#define left_end 0.1    //-90
+
+//memory opt
+// 5 degree seperate
+
+
+
+BX_servo::BX_servo(void){
+    
+    
+      angle = 0;
+
+      servo_in= new  PwmOut(PTB0);
+      
+      servo_in->period_ms(20);
+          
+      for(int i=0;i<37;i++){
+          
+          angle_level[i]=i*(0.05/36)+right_end;
+          }
+
+      *servo_in =angle_level[18];
+      
+    
+    }
+
+
+
+
+
+
+int BX_servo::set_angle(int a){
+    
+    
+      angle=a;
+       
+      *servo_in=angle_level[18+a/5];
+   
+         
+     
+    return angle;
+    
+    }
\ No newline at end of file