Diff: main.cpp

Revision:

1:a961c533bc91

Parent:

0:47edd2ca15c6

Child:

5:e1b1b9a43ccb

--- a/main.cpp	Thu May 08 12:26:51 2014 +0000
+++ b/main.cpp	Thu May 08 12:56:19 2014 +0000
@@ -3,10 +3,10 @@
 
 //pin set
 //Serial,I2C
-I2C i2c(p9, p10);        // sda, scl
-Serial pc(USBTX, USBRX); // tx, rx
-Serial gps(p28, p27);
-Serial xbee(p13,p14);
+I2C i2c(p9, p10);        //sda, scl,MFU
+Serial pc(USBTX, USBRX); //tx, rx
+Serial gps(p28, p27);    //tx, rx
+Serial xbee(p13,p14);    //tx,rx
 
 //motor
 DigitalOut motor1_A(p21);
@@ -30,7 +30,6 @@
 //Each value 
 char gyro_head[1];
 char read[8];
-
 short int gyro_x=0;
 short int gyro_y=0;
 short int gyro_z=0;
@@ -42,12 +41,13 @@
 short int acc_y = 0;
 short int acc_z = 0;
 
+//motor_speed_feed_back
 float target_palse = 0.0;
 float pwm;
-
 long encorder_count = 0;
 
 //GPS
+//Ring Buffer
 typedef unsigned char  UBYTE;
 typedef unsigned int UWORD;
 
@@ -62,7 +62,6 @@
 
 RINGP gps_buf;
 
-
 UBYTE get_ring(void){
     UBYTE result ;
     /* load data */
@@ -114,25 +113,25 @@
     acc_y = (acc_buf[3] << 8) + acc_buf[2];
     acc_z = (acc_buf[5] << 8) + acc_buf[4];
     
-    xbee.printf("%d, %d, %d ,", gyro_x, gyro_y, gyro_z);
+    //xbee.printf("%d, %d, %d ,", gyro_x, gyro_y, gyro_z);
     //xbee.printf("%d, %d, %d", acc_x, acc_y, acc_z);
-    xbee.printf("%ld,%lf,%lf\n",encorder_count,target_palse,pwm);
+    //xbee.printf("%ld,%lf,%lf\n",encorder_count,target_palse,pwm);
     
     
     //feedback
-    pwm = 0.01 * (target_palse - encorder_count/11264.0) + motor1_pwm;
-    motor1_pwm  = pwm;
-    encorder_count = 0;
+    //pwm = 0.01 * (target_palse - encorder_count/11264.0) + motor1_pwm;
+    //motor1_pwm  = pwm;
+    //encorder_count = 0;
     
-    
+    //Output GPS data -> xbee
     char val;
     gps_buf.rp = gps_buf.last_head_rp; 
-    
     while(1){
         val = get_ring();
         xbee.printf("%c",val);
         if(val == 0x0a) break;   
         }
+        
     }
     
 void gps_rx(){
@@ -146,19 +145,17 @@
     
 void xbee_rx(){
     char val = xbee.getc();
-    
     if(val == 'a') {
         motor1_A = 1;
         motor1_B = 0;
-        target_palse = 5.0;
+        motor1_pwm = 0.5;
         //motor2_A = 1;
         //motor2_B = 0;
         }
     else if(val == 's'){
         motor1_A = 0;
         motor1_B = 0;
-        target_palse = 0.0;
-        
+        motor1_pwm = 0.0;
         //motor2_A = 0;
         //motor2_B = 0;       
         }
@@ -208,7 +205,7 @@
     wait(5);
 
     //Encorder Innterrapt Setting
-    encoder1_A.rise(&flip);
+    //encoder1_A.rise(&flip);
     
     //gyro_registor Setting
     char PWR_M[2]={0x3E,0x80};
@@ -221,16 +218,12 @@
     i2c.write(gyro_addr, INT_C, 2); // Send commanad string
     char PWR_M2[2]={0x3E,0x00};
     i2c.write(gyro_addr, PWR_M2, 2); // Send command string
+    wait(1);
     
-    wait(1);
-    pc.printf("start\n");
+    //interrupt start
     axis.attach(&axisRenovation, 0.1);
-    
     motor1_pwm = 0.0;
-    //motor2_pwm = 0.3;        
-
     
     while (1) {
-
     }
 }
\ No newline at end of file