Revision 1:19d647492936, committed 2011-11-10

Comitter:

Keisuke_Fujii

Date:

Thu Nov 10 13:29:43 2011 +0000

Parent:

0:993cd673f077

Commit message:

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ClockControl/ClockControl/ClockControl.cpp	Thu Nov 10 13:29:43 2011 +0000
@@ -0,0 +1,37 @@
+#include "ClockControl.h"
+
+void setPLL0Frequency(unsigned char clkSrc, unsigned short M, unsigned char N)
+{
+      LPC_SC->CLKSRCSEL = clkSrc;
+      LPC_SC->PLL0CFG   = (((unsigned int)N-1) << 16) | M-1;
+      LPC_SC->PLL0CON   = 0x01;             
+      LPC_SC->PLL0FEED  = 0xAA;
+      LPC_SC->PLL0FEED  = 0x55;
+      while (!(LPC_SC->PLL0STAT & (1<<26)));
+    
+      LPC_SC->PLL0CON   = 0x03;
+      LPC_SC->PLL0FEED  = 0xAA;
+      LPC_SC->PLL0FEED  = 0x55;
+}
+
+void setPLL1Frequency(unsigned char clkSrc, unsigned short M, unsigned char N)
+{
+      LPC_SC->CLKSRCSEL = clkSrc;
+      LPC_SC->PLL1CFG   = (((unsigned int)N-1) << 16) | M-1;
+      LPC_SC->PLL1CON   = 0x01;             
+      LPC_SC->PLL1FEED  = 0xAA;
+      LPC_SC->PLL1FEED  = 0x55;
+      while (!(LPC_SC->PLL1STAT & (1<<26)));
+    
+      LPC_SC->PLL1CON   = 0x03;
+      LPC_SC->PLL1FEED  = 0xAA;
+      LPC_SC->PLL1FEED  = 0x55;
+}
+
+unsigned int setSystemFrequency(unsigned char clkDivider, unsigned char clkSrc, unsigned short M, unsigned char N)
+{
+    setPLL0Frequency(clkSrc, M, N);
+    LPC_SC->CCLKCFG = clkDivider - 1;
+    SystemCoreClockUpdate();
+    return SystemCoreClock;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ClockControl/ClockControl/ClockControl.h	Thu Nov 10 13:29:43 2011 +0000
@@ -0,0 +1,14 @@
+/* mbed PowerControl Library
+  * Copyright (c) 2010 Michael Wei
+  */ 
+  
+//shouldn't have to include, but fixes weird problems with defines
+#include "LPC1768/LPC17xx.h"
+
+#ifndef MBED_CLOCKCONTROL_H 
+#define MBED_CLOCKCONTROL_H 
+
+unsigned int setSystemFrequency(unsigned char clkDivider, unsigned char clkSrc, unsigned short M, unsigned char N);
+void setPLL0Frequency(unsigned char clkSrc, unsigned short M, unsigned char N);
+void setPLL1Frequency(unsigned char clkSrc, unsigned short M, unsigned char N);
+#endif
\ No newline at end of file

--- a/include_file.h	Mon Jun 13 07:41:53 2011 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,20 +0,0 @@
-#include "mbed.h"
-#include "QEI.h"
-
-//Serial  device( USBTX, USBRX );
-Serial  device( p28, p27 );
-QEI enco_right(  p5,  p6,  NC, 6 , QEI :: X4_ENCODING );
-QEI  enco_left( p17, p18,  NC, 6 , QEI :: X4_ENCODING );
-PwmOut   right_pwm( p22 );
-DigitalOut right_P( p7 );
-DigitalOut right_N( p8 );
-PwmOut    left_pwm( p21 );
-DigitalOut  left_P( p15 );
-DigitalOut  left_N( p16 );
-DigitalOut  enable( p19 );
-
-void move_f( float );
-void move_b( float );
-void turn_r( float );
-void turn_l( float );
-void   stop( void );
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/library.h	Thu Nov 10 13:29:43 2011 +0000
@@ -0,0 +1,39 @@
+#include         "mbed.h"
+#include          "QEI.h"
+#include "ClockControl.h"
+
+//Number of pulses in one revolution
+#define  PULSE 6144  //12(encorder_plse) * 16(gear_head) * 32(wheel)
+#define LENGTH    4  //servo_number
+
+Serial        device( p28, p27 );   //serial_PC
+Serial         servo( p9, p10 );    //serial_servo
+QEI            r_enc(  p6,  p5, NC, PULSE, QEI :: X4_ENCODING );    //encorder(right_motor)
+QEI            l_enc( p18, p17, NC, PULSE, QEI :: X4_ENCODING );    //encorder(left_motor)
+PwmOut     right_pwm( p22 );    //PWM(right_motor)
+PwmOut      left_pwm( p21 );    //PWM(left_motor)
+DigitalOut   right_P( p7 );     //H bridge(right_motor)
+DigitalOut   right_N( p8 );     //H bridge(right_motor)
+DigitalOut    left_P( p15 );    //H bridge(left_motor)
+DigitalOut    left_N( p16 );    //H bridge(left_motor)
+DigitalOut    enable( p19 );    //Enable_pin(motor_driver)
+PwmOut          led1( p23 );    //LED for observation of the robot
+PwmOut          led2( p24 );    //LED for observation of the robot
+
+//motor.h
+void    qei( void );
+void   move_f( double, double );
+void   move_b( double, double );
+void   turn_r( double, double );
+void   turn_l( double, double );
+void   stop( void );
+
+//servo.h
+short ID_num[LENGTH] = { 0x01, 0x02, 0x03, 0x04 };
+short degree[LENGTH] = { 0, 1500, -1500, 0 };
+
+void control_servo( void );
+void       cal_deg( short, short *, short * );
+void          save( short );
+short      cal_sum( short *, int );
+void        id_set( void );
\ No newline at end of file

--- a/main.cpp	Mon Jun 13 07:41:53 2011 +0000
+++ b/main.cpp	Thu Nov 10 13:29:43 2011 +0000
@@ -1,105 +1,29 @@
-#include "include_file.h"
+#include "library.h"
+#include   "motor.h"
+#include   "servo.h"
 
 int main( int argc, char **argv )
 {
-    char key;
+    char s;
     float d = 0.5;
+    
+    setSystemFrequency( 0x3, 0x1, 12, 1 ); // M=12, N=1, 96MHz
+        
     device.baud( 115200 );
+    servo.baud( 115200 );
+    led1.period_us( 10 );
+    led2.period_us( 10 );
     right_pwm.period_us( 10 );
     left_pwm.period_us( 10 );
-    enable.write( 1 );
+    enable.write( 1.0 );
+    led1.write( 1.0 );
+    led2.write( 1.0 );
     
-    device.putc('s');
-    device.printf("f:forward, b:back, r:turn_right\n"
-                  "l:turn_left, a:duty+=0.01, m:duty-=0.01\n");
-    while( 1 )
+    device.printf("Start:'0'\n");
+    while( s != 0 )
     {
-        key = device.getc();
-        switch( key )
-        {
-            case 'f':
-                move_f( d );
-                break;
-            case 'b':
-                move_b( d );
-                break;
-            case 'r':
-                turn_r( d );
-                break;
-            case 'l':
-                turn_l( d );
-                break;
-            case 's':
-                stop();
-                break;
-            case 'a':
-                d += 0.01;
-                break;
-            case 'm':
-                d -= 0.01;
-                break;
-            default:
-                break;
-        }
-        device.printf("duty = %1.2f\r", d);
-        wait_ms( 10 );
+        s = device.getc();
     }
-}
-
-void move_f( float d )
-{
-    right_P.write( 0 );
-    right_N.write( 1 );
-    left_P.write( 1 );
-    left_N.write( 0 );
-    right_pwm.write( d );
-    left_pwm.write( d );
-    /*right_pwm.pulsewidth_us( d );
-    left_pwm.pulsewidth_us( d );*/
-}
-
-void move_b( float d )
-{
-    right_P.write( 1 );
-    right_N.write( 0 );
-    left_P.write( 0 );
-    left_N.write( 1 );
-    right_pwm.write( d );
-    left_pwm.write( d );
-    /*right_pwm.pulsewidth_us( d );
-    left_pwm.pulsewidth_us( d );*/
-}
-
-void turn_r( float d )
-{
-    right_P.write( 0 );
-    right_N.write( 1 );
-    left_P.write( 0 );
-    left_N.write( 1 );
-    right_pwm.write( d );
-    left_pwm.write( d );
-    /*right_pwm.pulsewidth_us( d );
-    left_pwm.pulsewidth_us( d );*/
-}
-
-void turn_l( float d )
-{
-    right_P.write( 1 );
-    right_N.write( 0 );
-    left_P.write( 1 );
-    left_N.write( 0 );
-    right_pwm.write( d );
-    left_pwm.write( d );
-    /*right_pwm.pulsewidth_us( d );
-    left_pwm.pulsewidth_us( d );*/
-}
-
-void stop( void )
-{
-    right_P.write( 0 );
-    right_N.write( 0 );
-    left_P.write( 0 );
-    left_N.write( 0 );
-    right_pwm.write( 0 );
-    left_pwm.write( 0 );
+    //control_servo();
+    qei();
 }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.h	Thu Nov 10 13:29:43 2011 +0000
@@ -0,0 +1,104 @@
+void qei( void )
+{
+    char key;
+    double d1 = 0.5, d2 = 0.5;
+    device.printf("\nf:forward, b:back, r:turn_right\n"
+                  "l:turn_left, a:duty+=0.01, m:duty-=0.01\n");
+    
+    while( 1 )
+    {
+        /*device.printf( "Right_Pulses : %d\n", r_enc.getPulses() );
+        device.printf( "Right_Rotate : %f\n", (double)r_enc.getRevolutions() );
+        device.printf( " Left_Pulses : %d\n", l_enc.getPulses() );
+        device.printf( " Left_Rotate : %f\n", (double)l_enc.getRevolutions() );*/
+        key = device.getc();
+        switch( key )
+        {
+            case 'f':
+                move_f( d1, d2 );
+                break;
+            case 'b':
+                move_b( d1, d2 );
+                break;
+            case 'r':
+                turn_r( d1, d2 );
+                break;
+            case 'l':
+                turn_l( d1, d2 );
+                break;
+            case 's':
+                stop();
+                break;
+            case 'a':
+                d1 += 0.01;
+                d2 += 0.01;
+                break;
+            case 'm':
+                d1 -= 0.01;
+                d2 -= 0.01;
+                break;
+            default:
+                break;
+        }
+        device.printf("duty1 = %1.2f, duty2 = %1.2f\r", d1, d2);
+        wait_ms( 10 );
+    }
+}
+
+void move_f( double dr, double dl )
+{
+    right_P.write( 1 );
+    right_N.write( 0 );
+    left_P.write( 0 );
+    left_N.write( 1 );
+    right_pwm.write( dr );
+    left_pwm.write( dl );
+    /*right_pwm.pulsewidth_us( d );
+    left_pwm.pulsewidth_us( d );*/
+}
+
+void move_b( double dr, double dl )
+{
+    right_P.write( 0 );
+    right_N.write( 1 );
+    left_P.write( 1 );
+    left_N.write( 0 );
+    right_pwm.write( dr );
+    left_pwm.write( dl );
+    /*right_pwm.pulsewidth_us( d );
+    left_pwm.pulsewidth_us( d );*/
+}
+
+void turn_r( double dr, double dl )
+{
+    right_P.write( 1 );
+    right_N.write( 0 );
+    left_P.write( 1 );
+    left_N.write( 0 );
+    right_pwm.write( dr );
+    left_pwm.write( dl );
+    /*right_pwm.pulsewidth_us( d );
+    left_pwm.pulsewidth_us( d );*/
+}
+
+void turn_l( double dr, double dl )
+{
+    right_P.write( 0 );
+    right_N.write( 1 );
+    left_P.write( 0 );
+    left_N.write( 1 );
+    right_pwm.write( dr );
+    left_pwm.write( dl );
+    /*right_pwm.pulsewidth_us( d );
+    left_pwm.pulsewidth_us( d );*/
+}
+
+void stop( void )
+{
+    right_P.write( 0 );
+    right_N.write( 0 );
+    left_P.write( 0 );
+    left_N.write( 0 );
+    right_pwm.write( 0 );
+    left_pwm.write( 0 );
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servo.h	Thu Nov 10 13:29:43 2011 +0000
@@ -0,0 +1,81 @@
+void control_servo( void )
+{
+    short   torque[9]  = { 0xfa, 0xaf, 0x00, 0x00, 0x24, 0x01, 0x01, 0x01, 0x00 };
+    short send_deg[10] = { 0xfa, 0xaf, 0x00, 0x00, 0x1e, 0x02, 0x01, 0x01, 0x00, 0x00 };
+    //                      Hdr,  Hdr,   ID,  Flg,  Adr,  Len,  Cnt,Lower,Upper,  Sum
+    
+    for( int a = 0; a < LENGTH; a ++ )
+    {
+        torque[2] = ID_num[a];
+        torque[8] = cal_sum( torque, 8 );
+        for( int b = 0; b < 9; b ++ )   servo.putc( torque[b] );
+        wait_ms( 5 );
+    }
+    
+    while( 1 )
+    {
+        for( int a = 0; a < LENGTH; a ++ )
+        {
+            for( int i = 0; i < LENGTH; i ++ )
+            {
+                send_deg[2] = ID_num[i];
+                cal_deg( degree[a], &send_deg[7], &send_deg[8] );
+                send_deg[9] = cal_sum( send_deg, 9 );
+                for( int b = 0; b < 10; b ++ )   servo.putc( send_deg[b] );
+                wait_ms( 1000 );
+            }
+        }
+    }
+}
+
+void cal_deg( short degree, short *lower, short *upper )
+{
+    *lower = degree & 0x00ff;
+    *upper = ( degree >> 8 ) & 0x00ff;
+}
+
+void id_set( void )
+{
+    int i, j;
+    short ID[LENGTH][9] = 
+                         {
+                             { 0xfa, 0xaf, 0x01, 0x00, 0x04, 0x01, 0x01, 0x01, 0x00 }    //ID
+                         };
+    
+    for( i = 0; i < LENGTH; i ++ )
+    {
+        ID[i][7] = ID_num[i];
+        ID[i][8] = cal_sum( ID[i], 8 );
+        for( j = 0; j < 9; j ++ )   servo.putc( ID[i][j] );
+        save( ID[i][7] );
+    }
+}
+
+short cal_sum( short *data, int sum_num )
+{
+    int sum = 0, j;
+    
+    for( j = 2; j < sum_num; j ++ )
+    {
+        sum ^= ( int )data[j];
+    }
+    
+    return sum;
+}
+
+void save( short id )
+{
+    int i, j;
+    short save[2][8] =
+                    {
+                        { 0xfa, 0xaf, id, 0x40, 0xff, 0x00, 0x00, 0x00 },   //save
+                        { 0xfa, 0xaf, id, 0x20, 0xff, 0x00, 0x00, 0x00 }    //reboot
+                    };
+    
+    for( i = 0; i < 2; i ++ )
+    {
+        save[i][2] = id;
+        save[i][7] = cal_sum( save[i], 7 );
+        for( j = 0; j < 8; j ++ )   servo.putc( save[i][j] );
+    }
+}
\ No newline at end of file