Clark Lin
/
Boboobooo
QQQQQQQQQ
Fork of Boboobooo by
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