Revision 0:8f37781c0054, committed 2015-04-17

Comitter:

tzxl10000

Date:

Fri Apr 17 21:13:44 2015 +0000

Commit message:

Fish_2014 with BBBCamshift

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3G4200D.lib	Fri Apr 17 21:13:44 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/shimniok/code/L3G4200D/#14914cd8fdf3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3GD20.lib	Fri Apr 17 21:13:44 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/bclaus/code/L3GD20/#62dfce144cf7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLHC.lib	Fri Apr 17 21:13:44 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/bclaus/code/LSM303DLHC/#612f7d5a822d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLM.lib	Fri Apr 17 21:13:44 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/shimniok/code/LSM303DLM/#0fcea8569714

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PwmIn.lib	Fri Apr 17 21:13:44 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/PwmIn/#6d68eb9b6bbb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Fri Apr 17 21:13:44 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/iSerial/RingBuffer/RingBuffer.cpp	Fri Apr 17 21:13:44 2015 +0000
@@ -0,0 +1,73 @@
+//
+//  RingBuffer.cpp ... General purpose ring buffer library
+//
+//  Copyright 2012  Yoji KURODA
+//
+//  2009.11.13 ... Originally written in C by Y.Kuroda for Renesas H83664
+//  2012.08.31 ... Code convert for mbed in C++
+//
+
+#include "string.h"
+#include "RingBuffer.h"
+
+
+/*
+ *    Machine Independent Area
+ */
+RingBuffer::RingBuffer(int _bufsize)
+:   bufsize(_bufsize)
+{
+    buf = new unsigned char [bufsize+1];
+
+    sp = ep = (unsigned int)buf;
+    memset(buf,0,bufsize);
+}
+
+RingBuffer::~RingBuffer()
+{
+    delete [] buf;
+}
+
+int
+RingBuffer::save(unsigned char c)
+{
+    if( (ep==sp-1)||
+        ((sp==(unsigned int)buf)&&
+            (ep==(unsigned int)buf+bufsize-1)) )    /* buffer full */
+        return 0;
+
+    *(unsigned char*)ep = c;
+    ep++;
+
+    if(ep > (unsigned int)buf+bufsize)
+        ep = (unsigned int)buf;
+    return 1;
+}
+
+unsigned char
+RingBuffer::read(void)
+{
+    unsigned char ret;
+
+    if(sp == ep)
+        return 0;    /* buffer empty */
+
+    ret = *(unsigned char*)sp;
+    *(unsigned char*)sp = 0;
+    sp++;
+
+    if(sp> (unsigned int)buf+bufsize)
+        sp = (unsigned int)buf;
+    return ret;
+}
+
+int
+RingBuffer::check(void)
+{
+    int n = ep-sp;
+    if(n<0)
+        n = bufsize-n;
+
+    return n;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/iSerial/RingBuffer/RingBuffer.h	Fri Apr 17 21:13:44 2015 +0000
@@ -0,0 +1,35 @@
+//
+//  RingBuffer.h ... General purpose ring buffer library
+//
+//  Copyright 2012  Yoji KURODA
+//
+//  2009.11.13 ... Originally written in C by Y.Kuroda for Renesas H83664
+//  2012.08.31 ... Code convert for mbed in C++
+//
+
+#ifndef _RINGBUFFER_H
+#define _RINGBUFFER_H
+
+class RingBuffer {
+
+  protected:
+    unsigned char* buf;
+    unsigned int sp;
+    unsigned int ep;
+    int bufsize;
+
+  public:
+    RingBuffer(int _bufsize=100);
+    ~RingBuffer();
+    
+    int save(unsigned char c);
+    unsigned char read(void);
+    int check(void);
+    int buffersize(void){return bufsize;};
+    int full(void){ return (check()>=bufsize-1 ? 1 : 0); };
+};
+
+
+#endif /* _RINGBUFFER_H */
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/iSerial/iSerial.cpp	Fri Apr 17 21:13:44 2015 +0000
@@ -0,0 +1,205 @@
+//
+//  iSerial.cpp ... Serial Driver with Interrupt Rec/Send
+//
+//  2009.11.13 ... Originally written by Y.Kuroda for Renesas H83664
+//  2012.08.31 ... Code convert for mbed in C++
+//
+#include <stdarg.h>
+#include "mbed.h"
+#include "RingBuffer.h"
+#include "iSerial.h"
+
+//DigitalOut led3(LED3);
+//DigitalOut led4(LED4);
+
+
+void
+iSerial::enable_uart_irq(void)
+{
+    switch(tx){
+    case USBTX:
+        #if defined(TARGET_LPC1768)
+            NVIC_EnableIRQ(UART2_IRQn);
+        #elif defined(TARGET_LPC11U24)
+            NVIC_EnableIRQ(UART_IRQn);
+        #endif
+//        led1 = !led1;
+        break;
+        
+    case p9:
+        #if defined(TARGET_LPC1768)
+            NVIC_EnableIRQ(UART1_IRQn);
+        #elif defined(TARGET_LPC11U24)
+            NVIC_EnableIRQ(UART_IRQn);
+        #endif
+        break;
+
+    #if defined(TARGET_LPC1768)
+    case p13:
+        NVIC_EnableIRQ(UART3_IRQn);
+        break;
+    case p28:
+        NVIC_EnableIRQ(UART0_IRQn);
+        break;
+    #endif
+    }
+}
+
+void
+iSerial::disable_uart_irq(void)
+{
+    switch(tx){
+    case USBTX:
+        #if defined(TARGET_LPC1768)
+            NVIC_DisableIRQ(UART2_IRQn);
+        #elif defined(TARGET_LPC11U24)
+            NVIC_DisableIRQ(UART_IRQn);
+        #endif
+//        led1 = !led1;
+        break;
+        
+    case p9:
+        #if defined(TARGET_LPC1768)
+            NVIC_DisableIRQ(UART1_IRQn);
+        #elif defined(TARGET_LPC11U24)
+            NVIC_DisableIRQ(UART_IRQn);
+        #endif
+        break;
+
+    #if defined(TARGET_LPC1768)
+    case p13:
+        NVIC_DisableIRQ(UART3_IRQn);
+        break;
+    case p28:
+        NVIC_DisableIRQ(UART0_IRQn);
+        break;
+    #endif
+    }
+}
+
+/*
+ *    Interrupt Function
+ */
+void
+iSerial::rx_handler(void)
+{
+//  led3 = 1;
+    while(Serial::readable())
+        rxbuf.save(Serial::getc());
+//  led3 = 0;
+}
+
+void
+iSerial::tx_handler(void)
+{
+//  led4 = 1;
+    while(Serial::writeable() && txbuf.check())
+        Serial::putc( txbuf.read() );
+//  led4 = 0;
+}
+
+iSerial::iSerial(PinName _tx, PinName _rx, int _txbufsize, int _rxbufsize)
+:   Serial(_tx, _rx),
+    tx(_tx),
+    rx(_rx),
+    txbufsize(_txbufsize),
+    rxbufsize(_rxbufsize),
+    txbuf(RingBuffer(txbufsize)),
+    rxbuf(RingBuffer(rxbufsize)),
+    str(new char [txbufsize])    
+{
+    __disable_irq();
+
+    attach(this, &iSerial::tx_handler, Serial::TxIrq);
+    attach(this, &iSerial::rx_handler, Serial::RxIrq);
+
+//  format(8,Serial::None,1);   // default
+//  baud(baudrate);
+
+    __enable_irq();
+    enable_uart_irq();    
+}
+
+iSerial::~iSerial()
+{
+    delete [] str;
+}
+
+int
+iSerial::readable(void)
+{
+    return rxbuf.check();
+}
+
+int
+iSerial::getc(void)
+{
+    unsigned short int c;
+
+    while(!rxbuf.check());   // wait receiving a character
+    disable_uart_irq();
+    c = rxbuf.read();
+    enable_uart_irq();
+
+    return c;
+}
+
+void
+iSerial::putc(short ch)
+{
+    if(txbuf.check()==0 && Serial::writeable()){
+        Serial::putc(ch);
+        
+    } else {
+        while(txbuf.full()){
+            disable_uart_irq();
+            tx_handler();
+            enable_uart_irq();
+        }
+        
+        disable_uart_irq();
+        txbuf.save(ch);
+        enable_uart_irq();
+    }
+}
+
+short int
+iSerial::putstr(const char* s)
+{
+    int i=0;
+    for(; ; i++){
+        if(*s==0) break;
+        putc(*s++);
+    }
+    return i;
+}
+
+short int
+iSerial::puts(const char* s)
+{
+    short int n = putstr(s);
+    putc(CR);
+    putc(LF);
+    return n;
+}
+
+char*
+iSerial::printf(const char* format, ...)
+{
+    va_list arg;
+    va_start(arg,format);
+    vsprintf(str, format, arg);
+    va_end(arg);
+    putstr(str);
+    return str;
+}
+
+
+void
+iSerial::flush(void)
+{
+    while(txbuf.check())
+        enable_uart_irq();
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/iSerial/iSerial.h	Fri Apr 17 21:13:44 2015 +0000
@@ -0,0 +1,52 @@
+//
+//  iSerial.h ... Serial Driver with Interrupt Rec/Send
+//
+//  Copyright 2012  Yoji KURODA
+//
+//  2009.11.13 ... Originally written by Y.Kuroda for Renesas H83664
+//  2012.08.31 ... Code convert for mbed in C++
+//
+#ifndef _ISERIAL_H
+#define _ISERIAL_H
+
+#include <string.h>
+#include "RingBuffer.h"
+
+
+class iSerial : public Serial {
+  protected:
+
+    PinName tx;
+    PinName rx;
+    const int txbufsize;
+    const int rxbufsize;
+    RingBuffer txbuf;
+    RingBuffer rxbuf;
+    char* str;
+
+    void tx_handler(void);
+    void rx_handler(void);
+    void enable_uart_irq(void);
+    void disable_uart_irq(void);
+
+  public:
+
+    enum TERMINL_CODES { CR=0x0D, LF=0x0A };
+
+    iSerial(PinName _tx, PinName _rx, int _txbufsize=100, int _rxbufsize=100);
+    virtual ~iSerial();
+    
+    short int putstr(const char* s);
+
+    int readable(void);
+    int getc(void);
+    void putc(short ch);
+    short int puts(const char* s);
+    //void printf();
+    char* printf(const char* format, ...);
+
+    void flush(void);
+};
+
+
+#endif    /* _SCI_H */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 17 21:13:44 2015 +0000
@@ -0,0 +1,189 @@
+#include "mbed.h"
+#include "L3GD20.h"
+#include "LSM303DLHC.h"
+#include "Servo.h"
+#include "math.h"
+#include "PwmIn.h"
+
+#include "iSerial.h"
+#include "iostream"
+#include "stdio.h"
+#include "string.h"
+
+#include <string>
+#include <sstream>
+#define TS 10                    // sample time [ms]
+#define SIMULATION_TIME 10        // simulation time [s]
+
+
+
+//serial 
+Serial BB(p13, p14);  // tx, rx
+Serial pc(USBTX,USBRX);
+
+//period_pwm setup as 20ms
+//speed: 0 is off, and 1 is full speed；for servo 0-0.2 duty circle
+//direction: 0 clockwise, 1 counter-clockwise
+
+float period_pwm_ac; // Actuator current
+float period_pwm;
+
+bool directionA;
+bool directionB;
+bool directionC;
+
+float speedA;
+float speedB;
+float speedC;
+
+int flag;
+int x,y; // Image coordinate
+
+//funtion declaration
+void move(int motor, float speed, int direction);
+PwmOut PWMA(p25);//LEFT  FIN
+PwmOut PWMB(p23);//RIGHT FIN
+PwmOut PWMC(p24);//Left servo
+PwmOut PWMD(p26);//Right servo
+PwmOut PWME(p21);//Tail
+
+DigitalOut STBY (p30);
+//DigitalOut STBY2 (p29);
+DigitalOut AIN1 (p8);
+DigitalOut AIN2 (p11); // Fin1 left
+DigitalOut BIN1 (p7);  
+DigitalOut BIN2 (p6);  // Fin2 right
+DigitalOut myLed (LED1);
+DigitalOut myLed1(LED2);
+
+
+ 
+
+void moveA()
+{
+    STBY=1; //disable standby
+    int inPin1=1;
+    int inPin2=0;
+    if(directionA==0) {
+        inPin1 = 0;
+        inPin2 = 1;
+    }
+    AIN1=inPin1;
+    AIN2=inPin2;
+    directionA=!directionA;
+
+    //pc.printf("dirA = %d\n\r",directionA);
+
+
+
+    PWMA.pulsewidth(period_pwm*speedA);
+
+}
+
+void moveB()
+{
+    STBY=1; //disable standby
+    int inPin1=0;
+    int inPin2=1;
+    if(directionB==0) {
+        inPin1 = 1;
+        inPin2 = 0;
+    }
+    //pc.printf("dirB = %d\n\r",directionB);
+    BIN1=inPin1;
+    BIN2=inPin2;
+    directionB=!directionB;
+
+    PWMB.pulsewidth(period_pwm*speedB);
+
+}
+
+void stop()
+{
+//enable standby
+    STBY=0;;
+}
+
+int main()
+{
+    //serial to BBB setup    
+    char str[9];
+    char *token;
+ 
+    pc.baud(9600); 
+    BB.baud(9600); 
+    
+    //Actuator & servo setup
+    
+        period_pwm_ac=0.0020;                   //500hz
+        period_pwm=0.020;                      //20ms
+        PWMA.period(period_pwm_ac);          
+        PWMB.period(period_pwm_ac);          
+        PWMC.period(period_pwm);                // servo requires a 20ms period
+        PWMD.period(period_pwm);                // servo requires a 20ms period
+        PWME.period(period_pwm);                // servo requires a 20ms period
+        
+    //initial direction & current    
+        directionA=1;
+        directionB=0;
+        flag=1; 
+        speedA=0.2;
+        speedB=0.2; //Actuator speed control
+        
+        while(1)
+{
+            
+    //Talk to BBB
+         int i;
+         if(BB.readable()>0) 
+         {
+            for(i=0;i<9;i++) 
+            str[i] = BB.getc();
+           
+            if((0x30<str[1]<35)&&(str[8]==0x0D)&&(str[3]==0x2C))
+            {
+                token = strtok(str, ",");
+                x = atoi(token);                //100-420(width_320);No target:555 
+    
+                token = strtok(NULL, ",");
+                y = atoi(token);                //100-340(height_240); No target:555 
+               // pc.printf(" x=%d y=%d", x,y);
+              //pc.printf(str);
+            }
+         }
+         
+    //Serial test
+    if(x>210)
+    moveA();
+    else if (x<210)
+    moveB();
+    else if (x==555)
+    {
+         moveA();
+         moveB();
+         wait(0.2);
+    }
+
+
+//Actuator test
+//       moveA();
+//       moveB();
+//       wait(0.2);
+
+//Servo test
+         
+ //           PWMC.pulsewidth(period_pwm*0.08);
+ //           PWMD.pulsewidth(period_pwm*0.08);
+ //           PWME.pulsewidth(period_pwm*0.08);
+ //            wait(0.05);
+ //           PWMC.pulsewidth(period_pwm*0.05);
+ //           PWMD.pulsewidth(period_pwm*0.05);
+  //          PWME.pulsewidth(period_pwm*0.08);
+ //          
+ 
+
+        
+
+}
+            
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Apr 17 21:13:44 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/4fc01daae5a5
\ No newline at end of file