Diff: main.cpp

Revision:

0:47edd2ca15c6

Child:

1:a961c533bc91

Child:

2:f8bfd160f174

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 08 12:26:51 2014 +0000
@@ -0,0 +1,236 @@
+#include "mbed.h"
+#define BUFFER_SIZE 512
+
+//pin set
+//Serial,I2C
+I2C i2c(p9, p10);        // sda, scl
+Serial pc(USBTX, USBRX); // tx, rx
+Serial gps(p28, p27);
+Serial xbee(p13,p14);
+
+//motor
+DigitalOut motor1_A(p21);
+DigitalOut motor1_B(p22);
+PwmOut motor1_pwm(p23);
+
+DigitalOut motor2_A(p24);
+DigitalOut motor2_B(p25);
+PwmOut motor2_pwm(p26);
+
+//encorder
+InterruptIn encoder1_A(p15);
+InterruptIn encoder1_B(p16);
+
+Ticker axis;
+
+//I2C address 9-axis
+const int gyro_addr = 0xD0;
+const int acc_addr = 0xA6;
+
+//Each value 
+char gyro_head[1];
+char read[8];
+
+short int gyro_x=0;
+short int gyro_y=0;
+short int gyro_z=0;
+short int tempr=0;
+    
+char acc_head[1];
+char acc_buf[6];
+short int acc_x = 0;
+short int acc_y = 0;
+short int acc_z = 0;
+
+float target_palse = 0.0;
+float pwm;
+
+long encorder_count = 0;
+
+//GPS
+typedef unsigned char  UBYTE;
+typedef unsigned int UWORD;
+
+typedef struct {
+  UWORD cap;
+  UWORD wp;
+  UWORD last_head_rp;
+  UWORD head_rp;
+  UWORD rp;
+  UBYTE dat[BUFFER_SIZE];
+} RINGP;
+
+RINGP gps_buf;
+
+
+UBYTE get_ring(void){
+    UBYTE result ;
+    /* load data */
+    result = gps_buf.dat[gps_buf.rp] ;
+    /* update pointer */
+    gps_buf.rp += 1 ;
+    if ( gps_buf.rp == BUFFER_SIZE ) { gps_buf.rp = 0 ; }
+    /* capacity decrement */
+    gps_buf.cap -= 1 ;
+
+    return result ;
+}
+
+void put_ring(UBYTE x){
+    /* store data */
+    gps_buf.dat[gps_buf.wp] = x ;
+    /* update pointer */
+    gps_buf.wp += 1 ;
+    if ( gps_buf.wp == BUFFER_SIZE ) { gps_buf.wp = 0 ; }
+    /* capacity increment */
+    gps_buf.cap += 1 ;
+}
+
+void init_ring(void){
+    gps_buf.cap=0;
+    gps_buf.wp=0;
+    gps_buf.last_head_rp=0;
+    gps_buf.head_rp=0;
+    gps_buf.rp=0;
+}
+
+void axisRenovation(){
+    //gyro
+    gyro_head[0] = 0x1B;
+    i2c.write(gyro_addr, gyro_head, 1);
+    i2c.read(gyro_addr, read, 8);
+
+    tempr=(read[0] << 8) + read[1];
+    gyro_x=(read[2] << 8) + read[3];
+    gyro_y=(read[4] << 8) + read[5];
+    gyro_z=(read[6] << 8) + read[7];
+        
+    //acc
+    acc_head[0] = 0x32;
+    i2c.write(acc_addr,acc_head,1);
+    i2c.read(acc_addr, acc_buf, 6);
+        
+    acc_x = (acc_buf[1] << 8) + acc_buf[0];
+    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", acc_x, acc_y, acc_z);
+    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;
+    
+    
+    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(){
+    char val = gps.getc();
+    put_ring(val); 
+    if(val == '$'){
+        gps_buf.last_head_rp = gps_buf.head_rp;
+        gps_buf.head_rp = gps_buf.wp;
+        }    
+    }
+    
+void xbee_rx(){
+    char val = xbee.getc();
+    
+    if(val == 'a') {
+        motor1_A = 1;
+        motor1_B = 0;
+        target_palse = 5.0;
+        //motor2_A = 1;
+        //motor2_B = 0;
+        }
+    else if(val == 's'){
+        motor1_A = 0;
+        motor1_B = 0;
+        target_palse = 0.0;
+        
+        //motor2_A = 0;
+        //motor2_B = 0;       
+        }
+    else if(val == 'd'){    
+    target_palse = target_palse + 0.1;
+    }
+     else if(val == 'f'){    
+    target_palse = target_palse - 0.1;
+    }   
+        
+    
+    }
+
+void send_GPS_command(){
+    char gps_command1[] = "$PMTK220,500*2B";
+    char gps_command2[] = "$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29";
+    
+    char CR = 0x0d;
+    char LF = 0x0a;
+
+    gps.printf("%s%c%c",gps_command1,CR,LF);
+    gps.printf("%s%c%c",gps_command2,CR,LF);
+    
+    }
+
+
+void flip(){
+    encorder_count++;
+    }
+
+
+int main() {
+    //start up time
+    wait(1);
+    //set i2c frequency to 400 KHz
+    i2c.frequency(400000); 
+    //set pc frequency to 57600bps 
+    pc.baud(57600); 
+    //set xbee frequency to 57600bps
+    xbee.baud(57600);    
+    
+    //GPS setting
+    send_GPS_command();
+    init_ring();
+    gps.attach(gps_rx,Serial::RxIrq);
+    xbee.attach(xbee_rx,Serial::RxIrq);
+    wait(5);
+
+    //Encorder Innterrapt Setting
+    encoder1_A.rise(&flip);
+    
+    //gyro_registor Setting
+    char PWR_M[2]={0x3E,0x80};
+    i2c.write(gyro_addr, PWR_M, 2); // Send command string
+    char SMPL[2]={0x15,0x00};
+    i2c.write(gyro_addr, SMPL, 2); // Send command string
+    char DLPF[2]={0x16,0x18};
+    i2c.write(gyro_addr, DLPF, 2); // Send command string
+    char INT_C[2]={0x17,0x05};
+    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);
+    pc.printf("start\n");
+    axis.attach(&axisRenovation, 0.1);
+    
+    motor1_pwm = 0.0;
+    //motor2_pwm = 0.3;        
+
+    
+    while (1) {
+
+    }
+}
\ No newline at end of file