Revision 0:b5d91230ff3c, committed 2013-08-10

Comitter:

jksoft

Date:

Sat Aug 10 04:13:15 2013 +0000

Commit message:

Rev1

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HighSpeedAnalogIn/HighSpeedAnalogIn.cpp	Sat Aug 10 04:13:15 2013 +0000
@@ -0,0 +1,292 @@
+
+#include "HighSpeedAnalogIn.h"
+
+HighSpeedAnalogIn *HighSpeedAnalogIn::instance;
+int HighSpeedAnalogIn::refcnt = 0;
+
+HighSpeedAnalogIn::HighSpeedAnalogIn(PinName pin0, PinName pin1, PinName pin2, PinName pin3, PinName pin4, PinName pin5) {
+
+    refcnt++;
+    if (refcnt > 1) {
+        error("Please do not use over an object.");
+    }
+
+    static const int sample_rate = 200000;
+    static const int cclk_div = 1;
+
+    int adc_clk_freq = CLKS_PER_SAMPLE * sample_rate;
+    int m = (LPC_SC->PLL0CFG & 0xFFFF) + 1;
+    int n = (LPC_SC->PLL0CFG >> 16) + 1;
+    int cclkdiv = LPC_SC->CCLKCFG + 1;
+    int Fcco = (2 * m * XTAL_FREQ) / n;
+    int cclk = Fcco / cclkdiv;
+
+    LPC_SC->PCONP |= (1 << 12);
+    LPC_SC->PCLKSEL0 &= ~(0x3 << 24);
+    switch (cclk_div) {
+        case 1:
+            LPC_SC->PCLKSEL0 |= 0x1 << 24;
+            break;
+        case 2:
+            LPC_SC->PCLKSEL0 |= 0x2 << 24;
+            break;
+        case 4:
+            LPC_SC->PCLKSEL0 |= 0x0 << 24;
+            break;
+        case 8:
+            LPC_SC->PCLKSEL0 |= 0x3 << 24;
+            break;
+        default:
+            fprintf(stderr, "Warning: ADC CCLK clock divider must be 1, 2, 4 or 8. %u supplied.\n", cclk_div);
+            fprintf(stderr, "Defaulting to 1.\n");
+            LPC_SC->PCLKSEL0 |= 0x1 << 24;
+            break;
+    }
+    int pclk = cclk / cclk_div;
+    int clock_div = pclk / adc_clk_freq;
+
+    if (clock_div > 0xFF) {
+        fprintf(stderr, "Warning: Clock division is %u which is above 255 limit. Re-Setting at limit.\n", clock_div);
+        clock_div = 0xFF;
+    }
+    if (clock_div == 0) {
+        fprintf(stderr, "Warning: Clock division is 0. Re-Setting to 1.\n");
+        clock_div = 1;
+    }
+
+    int _adc_clk_freq = pclk / clock_div;
+    if (_adc_clk_freq > MAX_ADC_CLOCK) {
+        fprintf(stderr, "Warning: Actual ADC sample rate of %u which is above %u limit\n", _adc_clk_freq / CLKS_PER_SAMPLE, MAX_ADC_CLOCK / CLKS_PER_SAMPLE);
+        int max_div = 1;
+        while ((pclk / max_div) > MAX_ADC_CLOCK) {
+            max_div++;
+        }
+        fprintf(stderr, "Maximum recommended sample rate is %u\n", (pclk / max_div) / CLKS_PER_SAMPLE);
+    }
+
+    LPC_ADC->ADCR = ((clock_div - 1) << 8) | (1 << 21);
+    LPC_ADC->ADCR &= ~0xFF;
+
+    for (int i = 0; i < 8; i++) {
+        _adc_data[i] = 0;
+    }
+
+    // Attach IRQ
+    instance = this;
+    NVIC_SetVector(ADC_IRQn, (uint32_t)&static_adcisr);
+
+    // Disable global interrupt
+    LPC_ADC->ADINTEN &= ~0x100;
+
+    // Clock frequency.
+    printf("Clock frequency:%d\n", _adc_clk_freq);
+
+    // Actual sampling rate.
+    printf("Actual sampling rate:%d\n", _adc_clk_freq / CLKS_PER_SAMPLE);
+    
+    int tmp = LPC_ADC->ADCR & ~(0x0F << 24);
+    tmp |= ((0x0 & 7) << 24) | ((0x0 & 1) << 27);
+    LPC_ADC->ADCR = tmp;
+    LPC_ADC->ADCR |= (1 << 16);
+
+    if (pin0 != NC) setup(pin0, 1);
+    if (pin1 != NC) setup(pin1, 1);
+    if (pin2 != NC) setup(pin2, 1);
+    if (pin3 != NC) setup(pin3, 1);
+    if (pin4 != NC) setup(pin4, 1);
+    if (pin5 != NC) setup(pin5, 1);
+
+    interrupt_state(pin0, 1);
+}
+
+HighSpeedAnalogIn::~HighSpeedAnalogIn() {
+}
+
+void HighSpeedAnalogIn::static_adcisr(void) {
+    instance->adcisr();
+}
+
+void HighSpeedAnalogIn::adcisr(void) {
+    uint32_t stat = LPC_ADC->ADSTAT;
+    // Scan channels for over-run or done and update array
+    if (stat & 0x0101) _adc_data[0] = LPC_ADC->ADDR0;
+    if (stat & 0x0202) _adc_data[1] = LPC_ADC->ADDR1;
+    if (stat & 0x0404) _adc_data[2] = LPC_ADC->ADDR2;
+    if (stat & 0x0808) _adc_data[3] = LPC_ADC->ADDR3;
+    if (stat & 0x1010) _adc_data[4] = LPC_ADC->ADDR4;
+    if (stat & 0x2020) _adc_data[5] = LPC_ADC->ADDR5;
+    if (stat & 0x4040) _adc_data[6] = LPC_ADC->ADDR6;
+    if (stat & 0x8080) _adc_data[7] = LPC_ADC->ADDR7;
+}
+
+int HighSpeedAnalogIn::get_channel(PinName pin) {
+    int ch;
+    switch (pin) {
+        case p15:// =p0.23 of LPC1768
+            ch = 0;
+            break;
+        case p16:// =p0.24 of LPC1768
+            ch = 1;
+            break;
+        case p17:// =p0.25 of LPC1768
+            ch = 2;
+            break;
+        case p18:// =p0.26 of LPC1768
+            ch = 3;
+            break;
+        case p19:// =p1.30 of LPC1768
+            ch = 4;
+            break;
+        case p20:// =p1.31 of LPC1768
+            ch = 5;
+            break;
+        default:
+            ch = 0;
+            break;
+    }
+    return ch;
+}
+
+uint32_t HighSpeedAnalogIn::get_data(PinName pin) {
+    // If in burst mode and at least one interrupt enabled then
+    // take all values from _adc_data
+    if (LPC_ADC->ADINTEN & 0x3F) {
+        return (_adc_data[get_channel(pin)]);
+    } else {
+        // Return current register value or last value from interrupt
+        switch (pin) {
+            case p15:// =p0.23 of LPC1768
+                return ((LPC_ADC->ADINTEN & 0x01) ? _adc_data[0] : LPC_ADC->ADDR0);
+            case p16:// =p0.24 of LPC1768
+                return ((LPC_ADC->ADINTEN & 0x02) ? _adc_data[1] : LPC_ADC->ADDR1);
+            case p17:// =p0.25 of LPC1768
+                return ((LPC_ADC->ADINTEN & 0x04) ? _adc_data[2] : LPC_ADC->ADDR2);
+            case p18:// =p0.26 of LPC1768:
+                return ((LPC_ADC->ADINTEN & 0x08) ? _adc_data[3] : LPC_ADC->ADDR3);
+            case p19:// =p1.30 of LPC1768
+                return ((LPC_ADC->ADINTEN & 0x10) ? _adc_data[4] : LPC_ADC->ADDR4);
+            case p20:// =p1.31 of LPC1768
+                return ((LPC_ADC->ADINTEN & 0x20) ? _adc_data[5] : LPC_ADC->ADDR5);
+            default:
+                return 0;
+        }
+    }
+}
+
+// Enable or disable an HighSpeedAnalogIn pin
+void HighSpeedAnalogIn::setup(PinName pin, int state) {
+    int ch = get_channel(pin);
+    if ((state & 1) == 1) {
+        switch (pin) {
+            case p15:// =p0.23 of LPC1768
+                LPC_PINCON->PINSEL1 &= ~((unsigned int)0x3 << 14);
+                LPC_PINCON->PINSEL1 |= (unsigned int)0x1 << 14;
+                LPC_PINCON->PINMODE1 &= ~((unsigned int)0x3 << 14);
+                LPC_PINCON->PINMODE1 |= (unsigned int)0x2 << 14;
+                break;
+            case p16:// =p0.24 of LPC1768
+                LPC_PINCON->PINSEL1 &= ~((unsigned int)0x3 << 16);
+                LPC_PINCON->PINSEL1 |= (unsigned int)0x1 << 16;
+                LPC_PINCON->PINMODE1 &= ~((unsigned int)0x3 << 16);
+                LPC_PINCON->PINMODE1 |= (unsigned int)0x2 << 16;
+                break;
+            case p17:// =p0.25 of LPC1768
+                LPC_PINCON->PINSEL1 &= ~((unsigned int)0x3 << 18);
+                LPC_PINCON->PINSEL1 |= (unsigned int)0x1 << 18;
+                LPC_PINCON->PINMODE1 &= ~((unsigned int)0x3 << 18);
+                LPC_PINCON->PINMODE1 |= (unsigned int)0x2 << 18;
+                break;
+            case p18:// =p0.26 of LPC1768:
+                LPC_PINCON->PINSEL1 &= ~((unsigned int)0x3 << 20);
+                LPC_PINCON->PINSEL1 |= (unsigned int)0x1 << 20;
+                LPC_PINCON->PINMODE1 &= ~((unsigned int)0x3 << 20);
+                LPC_PINCON->PINMODE1 |= (unsigned int)0x2 << 20;
+                break;
+            case p19:// =p1.30 of LPC1768
+                LPC_PINCON->PINSEL3 &= ~((unsigned int)0x3 << 28);
+                LPC_PINCON->PINSEL3 |= (unsigned int)0x3 << 28;
+                LPC_PINCON->PINMODE3 &= ~((unsigned int)0x3 << 28);
+                LPC_PINCON->PINMODE3 |= (unsigned int)0x2 << 28;
+                break;
+            case p20:// =p1.31 of LPC1768
+                LPC_PINCON->PINSEL3 &= ~((unsigned int)0x3 << 30);
+                LPC_PINCON->PINSEL3 |= (unsigned int)0x3 << 30;
+                LPC_PINCON->PINMODE3 &= ~((unsigned int)0x3 << 30);
+                LPC_PINCON->PINMODE3 |= (unsigned int)0x2 << 30;
+                break;
+            default:
+                error("Invalid pin.");
+                break;
+        }
+        // Select channel
+        LPC_ADC->ADCR |= (1 << ch);
+    } else {
+        switch (pin) {
+            case p15://=p0.23 of LPC1768
+                LPC_PINCON->PINSEL1 &= ~((unsigned int)0x3 << 14);
+                LPC_PINCON->PINMODE1 &= ~((unsigned int)0x3 << 14);
+                break;
+            case p16://=p0.24 of LPC1768
+                LPC_PINCON->PINSEL1 &= ~((unsigned int)0x3 << 16);
+                LPC_PINCON->PINMODE1 &= ~((unsigned int)0x3 << 16);
+                break;
+            case p17://=p0.25 of LPC1768
+                LPC_PINCON->PINSEL1 &= ~((unsigned int)0x3 << 18);
+                LPC_PINCON->PINMODE1 &= ~((unsigned int)0x3 << 18);
+                break;
+            case p18://=p0.26 of LPC1768:
+                LPC_PINCON->PINSEL1 &= ~((unsigned int)0x3 << 20);
+                LPC_PINCON->PINMODE1 &= ~((unsigned int)0x3 << 20);
+                break;
+            case p19://=p1.30 of LPC1768
+                LPC_PINCON->PINSEL3 &= ~((unsigned int)0x3 << 28);
+                LPC_PINCON->PINMODE3 &= ~((unsigned int)0x3 << 28);
+                break;
+            case p20://=p1.31 of LPC1768
+                LPC_PINCON->PINSEL3 &= ~((unsigned int)0x3 << 30);
+                LPC_PINCON->PINMODE3 &= ~((unsigned int)0x3 << 30);
+                break;
+            default:
+                error("Invalid pin.");
+                break;
+        }
+        LPC_ADC->ADCR &= ~(1 << ch);
+    }
+}
+
+void HighSpeedAnalogIn::interrupt_state(PinName pin, int state) {
+    int ch = get_channel(pin);
+    if (state == 1) {
+        LPC_ADC->ADINTEN &= ~0x100;
+        LPC_ADC->ADINTEN |= 1 << ch;
+        /* Enable the HighSpeedAnalogIn Interrupt */
+        NVIC_EnableIRQ(ADC_IRQn);
+    } else {
+        LPC_ADC->ADINTEN &= ~(1 << ch);
+        //Disable interrrupt if no active pins left
+        if ((LPC_ADC->ADINTEN & 0xFF) == 0)
+            NVIC_DisableIRQ(ADC_IRQn);
+    }
+}
+
+float HighSpeedAnalogIn::read(PinName pin) {
+    /*
+     * Reset DONE and OVERRUN.
+     *
+     * bit 31 : DONE
+     * bit 30 : OVERRUN
+     */
+    _adc_data[get_channel(pin)] &= ~(((uint32_t)0x01 << 31) | ((uint32_t)0x01 << 30));
+    return (float)((get_data(pin) >> 4) & 0xFFF) / (float)0xFFF;
+}
+
+unsigned short HighSpeedAnalogIn::read_u16(PinName pin) {
+    /*
+     * Reset DONE and OVERRUN.
+     *
+     * bit 31 : DONE
+     * bit 30 : OVERRUN
+     */
+    _adc_data[get_channel(pin)] &= ~(((uint32_t)0x01 << 31) | ((uint32_t)0x01 << 30));
+    return ((get_data(pin) >> 4) & 0xFFF);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HighSpeedAnalogIn/HighSpeedAnalogIn.h	Sat Aug 10 04:13:15 2013 +0000
@@ -0,0 +1,35 @@
+#ifndef HIGH_SPEED_ANALOG_IN_H
+#define HIGH_SPEED_ANALOG_IN_H
+
+#include "mbed.h"
+
+class HighSpeedAnalogIn {
+public:
+
+    HighSpeedAnalogIn(PinName pin0, PinName pin1 = NC, PinName pin2 = NC, PinName pin3 = NC, PinName pin4 = NC, PinName pin5 = NC);
+    ~HighSpeedAnalogIn();
+    float read(PinName pin);
+    unsigned short read_u16(PinName pin);
+
+private:
+
+    HighSpeedAnalogIn();
+    uint32_t _adc_data[8];
+
+    static const int XTAL_FREQ = 12000000;
+    static const int MAX_ADC_CLOCK = 13000000;
+    static const int CLKS_PER_SAMPLE = 64;
+    
+    static HighSpeedAnalogIn *instance;
+    static int refcnt;
+
+    static void static_adcisr(void);
+
+    int get_channel(PinName pin);
+    uint32_t get_data(PinName pin);
+    void adcisr(void);
+    void setup(PinName pin, int state);
+    void interrupt_state(PinName pin, int state);
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TB6612/TB6612.cpp	Sat Aug 10 04:13:15 2013 +0000
@@ -0,0 +1,55 @@
+/**
+ * Motor Driver TB6612 Control Library
+ *
+ * -- TB6612 is a device of the TOSHIBA. 
+ *
+ * Copyright (C) 2012 Junichi Katsu (JKSOFT) 
+ */
+
+
+#include "TB6612.h"
+
+// TB6612 Class Constructor
+TB6612::TB6612(PinName pwm, PinName fwd, PinName rev):
+        _pwm(pwm), _fwd(fwd), _rev(rev) {
+
+    _fwd = 0;
+    _rev = 0;
+    _pwm = 0.0;
+    _pwm.period(0.001);
+}
+
+// Speed Control
+//  arg
+//   int speed -100 -- 0 -- 100
+void TB6612::speed(int speed) {
+        
+    if( speed > 0 )
+    {
+        _pwm = ((float)speed) / 100.0;
+        _fwd = 1;
+        _rev = 0;
+    }
+    else if( speed < 0 )
+    {
+        _pwm = -((float)speed) / 100.0;
+        _fwd = 0;
+        _rev = 1;
+    }
+    else
+    {
+        _fwd = 1;
+        _rev = 1;
+    }
+}
+
+
+// Speed Control with time-out
+//  arg
+//   int speed -100 -- 0 -- 100
+//   int time  0
+void TB6612::move(int sspeed , int time)
+{
+    speed(sspeed);
+    wait_ms(time);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TB6612/TB6612.h	Sat Aug 10 04:13:15 2013 +0000
@@ -0,0 +1,30 @@
+/**
+ * Motor Driver TB6612 Control Library
+ *
+ * -- TB6612 is a device of the rohm. 
+ *
+ * Copyright (C) 2012 Junichi Katsu (JKSOFT) 
+ */
+
+#ifndef MBED_TB6612_H
+#define MBED_TB6612_H
+
+#include "mbed.h"
+
+class TB6612 {
+public:
+    TB6612(PinName pwm, PinName fwd, PinName rev);
+    void speed(int speed);
+    void move(int speed , int time);
+    void operator= ( int value )
+    {
+        speed(value);
+    }
+    
+protected:
+    PwmOut _pwm;
+    DigitalOut _fwd;
+    DigitalOut _rev;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Aug 10 04:13:15 2013 +0000
@@ -0,0 +1,97 @@
+#include "mbed.h"
+#include "TB6612.h"
+#include "HighSpeedAnalogIn.h"
+
+#define SENSOR_BORDER   1000
+#define SENSOR_NUM      4
+#define MOTOR_MAX_VALUE 100
+#define MOTOR_MIN_VALUE -100
+
+#define KP 1
+#define KI 0
+#define KD 20
+
+BusOut sensor_on(LED1, LED2, LED3, LED4);
+
+TB6612 left(p21,p12,p11);
+TB6612 right(p22,p14,p13);
+
+HighSpeedAnalogIn sensor(p15, p16, p17, p18, p19, p20);
+
+DigitalIn sw(p29);
+Ticker tick;
+
+int motor_cnt_val = 0;
+
+void cycle()
+{
+  float sensor_val = 0;
+  float p_val,d_val,pid_val;
+  static float i_val,prev_sensor_val = 0;
+  int bit = 0;
+
+  if( sensor.read_u16(p15) > SENSOR_BORDER ) bit |= 0x01;
+  if( sensor.read_u16(p16) > SENSOR_BORDER ) bit |= 0x02;
+  if( sensor.read_u16(p17) > SENSOR_BORDER ) bit |= 0x04;
+  if( sensor.read_u16(p18) > SENSOR_BORDER ) bit |= 0x08;
+
+  sensor_on = bit;
+
+  switch(bit)
+  {
+  case 0x01:    sensor_val = -1.0;   break;
+  case 0x03:    sensor_val = -0.66;  break;
+  case 0x02:    sensor_val = -0.33;  break;
+  case 0x04:    sensor_val = 0.33;   break;
+  case 0x0C:    sensor_val = 0.66;   break;
+  case 0x08:    sensor_val = 1.0;    break;
+  default:      sensor_val = 0;      break;
+  }
+  
+  p_val = sensor_val;
+  i_val += p_val;
+  d_val = sensor_val - prev_sensor_val;
+  prev_sensor_val = sensor_val;
+  
+  pid_val = (p_val * (KP) ) + (i_val*(KI)) + (d_val*(KD));
+  
+  motor_cnt_val = (int)(pid_val * 100.0);
+}
+
+int main() {
+  sw.mode(PullUp);
+  
+  tick.attach(cycle,0.01);
+
+  while(sw==1)
+  {
+    int a[4] = {sensor.read_u16(p15),sensor.read_u16(p16),sensor.read_u16(p17),sensor.read_u16(p18)};
+    
+    printf("[0]:%05d  [0]:%05d  [0]:%05d  [0]:%05d\n\r",a[0],a[1],a[2],a[3]);
+    wait(0.1);
+  }
+
+  wait(1);
+
+  while(1) {
+
+    if( motor_cnt_val < 0 )
+    {
+      right = MOTOR_MAX_VALUE
+              + motor_cnt_val;
+    }
+    else
+    {
+      right = MOTOR_MAX_VALUE;
+    }
+    if( motor_cnt_val > 0 )
+    {
+      left = MOTOR_MAX_VALUE
+             - motor_cnt_val;
+    }
+    else
+    {
+      left = MOTOR_MAX_VALUE;
+    }
+  }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Aug 10 04:13:15 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/3d0ef94e36ec
\ No newline at end of file