Revision 1:a4ab99c0f7ea, committed 2013-05-29

Comitter:

takeuchi

Date:

Wed May 29 06:45:00 2013 +0000

Parent:

0:0522a96f04ed

Commit message:

Tamagawa IMU 3servo

Changed in this revision

diff -r 0522a96f04ed -r a4ab99c0f7ea main.cpp
--- a/main.cpp	Sun Jan 30 06:55:22 2011 +0000
+++ b/main.cpp	Wed May 29 06:45:00 2013 +0000
@@ -1,41 +1,195 @@
-// GPS GT-720F Test01
-#include "mbed.h"
-#include "TextLCD0420.h"
-
-#define ON 1
-#define OFF 0
-
-DigitalOut mled0(LED1);
-DigitalOut mled1(LED2);
-
-TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3
-Serial pc(USBTX, USBRX); // tx, rx 
-Serial gps(p9,p10);
-
-int main() {
-
-      unsigned char c;
-      int i;
-      
-      gps.baud(9600);
-      lcd.cls();
-      lcd.locate(0,0);
-      lcd.printf("GPS GT720F Test\n");
-
-    while (1) {
-      while(gps.getc()!='$'){
-      }
-      pc.printf("\x0D");// new line
-      pc.printf("Recive start:\n");
-      c=gps.getc();
-      while( c != '\r'){ //line end
-        pc.putc(c);
-        c=gps.getc();
-      }
-      pc.printf("\n");
-     
-    }//while
-}//main
-
-
-
+// Tamagawaseiki IMU Demo02
+// 3Servo Demonstration      
+#include "mbed.h"
+#include "TextLCD0420.h"
+
+#define ON 1
+#define OFF 0
+
+DigitalOut mled0(LED1);
+DigitalOut mled1(LED2);
+PwmOut servo1(p21);
+PwmOut servo2(p22);
+PwmOut servo3(p23);
+
+TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3
+Serial pc(USBTX, USBRX); // tx, rx 
+Serial IMU(p9,p10);// p9=tx,p10=rx
+
+double rx,px,ax;
+double rx_imu,px_imu,ax_imu; 
+
+void IMU_get(){
+      
+      unsigned char c1,c2;
+      int i;
+      
+      while(IMU.getc()!='$'){
+      }
+      for(i=1;i<=8;i++){
+        c1=IMU.getc();
+      }
+      for(i=1;i<=20;i++){
+        c1=IMU.getc();
+      }
+      c1=IMU.getc();
+      c2=IMU.getc();
+      rx_imu=float((c1 << 8) | c2)*180/32768;
+
+      c1=IMU.getc();
+      c2=IMU.getc();
+      px_imu=float((c1 << 8) | c2)*180/32768;
+
+      c1=IMU.getc();
+      c2=IMU.getc();
+      ax=float((c1 << 8) | c2)*180/32768;
+    
+    //for rx  
+      if( rx_imu >= 0 && rx_imu <= 90){
+        rx=rx_imu+90;
+      }
+      else if( rx > 90 && rx < 180){
+        //rx=180;
+      }
+      else if( rx >= 180 && rx < 270){
+        //rx=0;
+      }
+      else if(rx_imu >= 270 && rx_imu <= 360){
+        rx=rx_imu-270;
+      }
+      
+    //for px
+      if( px_imu >= 0 && px_imu <= 90){
+        px=px_imu+90;
+      }
+      else if( px > 90 && px < 180){
+        //px=180;
+      }
+      else if(px >=180 && px < 270){
+        //px=0;
+      }
+      else if(px_imu >= 270 && px_imu <= 360){
+        px=px_imu-270;
+      }
+      
+    // for ax
+      if(ax >= 0 && ax <=90){
+        ax=ax+90;
+      }
+      else if(ax >90 && ax <180){
+        ax=180;
+      }
+      else if(ax >= 180 && ax <270){
+        ax=0;
+      }
+      else if(ax >= 270 && ax <=360){
+        ax=ax-270;
+      }
+ 
+}
+
+int main() {
+
+      int i,j,k;
+      int rx_lc=9,rx_lc_old=9;
+      int px_lc=9,px_lc_old=9;
+      int ax_lc=9,ax_lc_old=9;
+      double rx_pwidth,px_pwidth,ax_pwidth;
+      
+      servo1.period_ms(20);
+      servo2.period_ms(20);
+      servo3.period_ms(20);
+      
+      //IMU.baud(57600);  
+      IMU.baud(119200);
+      lcd.cls();
+      lcd.locate(0,0);
+      lcd.printf("Tamagawaseiki IMU\n");
+      lcd.printf("3Servo Demo Ver.02\n");
+      lcd.printf("System start ");
+      
+      servo1.pulsewidth(1.5/1000);
+      servo2.pulsewidth(1.5/1000);
+      servo3.pulsewidth(1.5/1000);
+
+      //IMU.printf("$TSC,OFC,10*CC\r\n");                 
+ 
+      IMU.printf("$TSC,RAW,50\r\n");// IMU data output start
+      for(i=0;i<5;i++){
+        lcd.printf(".");
+        IMU.printf("$TSC,RAW,50\r\n");
+        wait(1.0);
+      } 
+      
+      IMU.printf("$TSC,HRST*75\r\n");// ax reset
+          
+      IMU_get(); 
+      lcd.cls();
+      wait(0.1);      
+            
+    while (1) {
+      IMU_get();
+    
+    //for rx  
+      if( rx >= 88 && rx <= 92){
+        rx=90;
+      }
+      if( rx >= 170 && rx < 180){
+        rx=170;
+      }
+      else if( rx <=10 && rx > 0){
+        rx=10;
+      }
+
+   //for px
+      if( px >= 88 && px <= 92){
+        px=90;
+      }   
+      if( px >= 130 ){
+        px=130;
+      }
+      else if(px < 50 ){
+        px=50;
+      }
+              
+      lcd.locate(0,0);
+      lcd.printf("%5.1f,%5.1f,%6.2f\n",rx,px,ax);
+
+      rx_lc=rx/10; 
+      px_lc=px/10;
+      ax_lc=ax/10;     
+ 
+      ax=-ax+180;//servo pulse reverse
+      
+      rx_pwidth=rx/180/1000+0.001;
+      px_pwidth=px/180/1000+0.001;
+      ax_pwidth=ax/180/1000+0.001;      
+      
+      servo1.pulsewidth(rx_pwidth);
+      servo2.pulsewidth(px_pwidth);
+      servo3.pulsewidth(ax_pwidth);
+      
+      lcd.locate(0,1);
+      //lcd.printf("---------+---------");
+      //lcd.printf("R:%4.2fms,P:%4.2fms",rx_pwidth*1000,px_pwidth*1000);    
+      lcd.locate(rx_lc_old,1);
+      lcd.printf(" ");
+      lcd.locate(rx_lc,1);
+      lcd.printf("*");
+      rx_lc_old=rx_lc;
+      lcd.locate(px_lc_old,2);
+      lcd.printf(" ");
+      lcd.locate(px_lc,2);
+      lcd.printf("*");
+      px_lc_old=px_lc;
+      lcd.locate(ax_lc_old,3);
+      lcd.printf(" ");
+      lcd.locate(ax_lc,3);
+      lcd.printf("*");
+      ax_lc_old=ax_lc;
+              
+    }//while
+}//main
+  
+
+

diff -r 0522a96f04ed -r a4ab99c0f7ea mbed.bld
--- a/mbed.bld	Sun Jan 30 06:55:22 2011 +0000
+++ b/mbed.bld	Wed May 29 06:45:00 2013 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da
+http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da
\ No newline at end of file