Raharja Agie
Read IMU data at Serial port (p28, p27), data rate <100Hz @115200bps Read 5 channels PPM signal at p11 using InterruptIn Send data via RF Modem at serial port (p13, p14) in 36Hz @115200 Drive i2C motor speed controller at I2C port (p9, p10) in every 10mS (control loop delay time)
Revision 0:7ccd56e1c3b1, committed 2010-08-11
- Comitter:
- agiembed
- Date:
- Wed Aug 11 01:23:42 2010 +0000
- Commit message:
Changed in this revision
diff -r 000000000000 -r 7ccd56e1c3b1 Set_up.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Set_up.h Wed Aug 11 01:23:42 2010 +0000
@@ -0,0 +1,14 @@
+
+Serial pc(USBTX, USBRX);
+Serial ahrs(p28, p27);
+Serial radio(p13, p14);
+Ticker SendData;
+Ticker Proccess;
+DigitalOut myled(LED1);
+
+void setup(){
+ pc.baud(115200);
+ radio.baud(115200);
+ ahrs.baud(115200);
+}
+
\ No newline at end of file
diff -r 000000000000 -r 7ccd56e1c3b1 ahrs.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ahrs.h Wed Aug 11 01:23:42 2010 +0000
@@ -0,0 +1,72 @@
+#define Ahrs_num 40
+#define start 0
+#define head1 0xFF
+#define head2 0xFF
+#define check1 1
+#define check2 2
+
+unsigned char buf[1000], head[2], q[41], ck[1], data_idx = 0;
+char AHRS[Ahrs_num], check_ahrs = start, checkdata = 0, checksum;
+
+
+void IMU_Update(){
+ fcc.roll = IMU.roll;
+ fcc.pitch = IMU.pitch;
+ fcc.yaw = IMU.yaw;
+ fcc.g_roll = IMU.g_roll;
+ fcc.g_pitch = IMU.g_pitch;
+ fcc.g_yaw = IMU.g_yaw;
+ fcc.acc_x = IMU.acc_x;
+ fcc.acc_y = IMU.acc_y;
+ fcc.acc_z = IMU.acc_z;
+}
+
+
+void Get_AHRS(char *data){
+ int i;
+ char *dTemp = (char *) &IMU;
+
+ for(i=0; i<Ahrs_num; i++) *(dTemp+i) = *(data+i);
+ IMU_Update();
+}
+
+void ahrs_data(unsigned char data){
+
+ if(check_ahrs == start){
+ if(data == head1) check_ahrs = check1;
+ else check_ahrs = start;
+ }
+
+ else if(check_ahrs == check1){
+ if(data == head2) check_ahrs = check2;
+ else check_ahrs = start;
+ }
+
+ else if(check_ahrs == check2){
+ AHRS[data_idx] = data;
+ if(data_idx<(Ahrs_num-1)){
+ //checkdata ^= AHRS[data_idx];
+ data_idx++;
+ if(data_idx == 3) data_idx = 4;
+ }
+ else{
+ checksum = data;
+ //if(checksum == checkdata)
+ Get_AHRS(AHRS);
+ data_idx = 0;
+ check_ahrs = start;
+ }
+ }
+
+ else{
+ data_idx = 0;
+ check_ahrs = start;
+ }
+}
+
+void ahrs_rec(){
+ unsigned char data;
+ data = ahrs.getc();
+ ahrs_data(data);
+ //pc.putc(data);
+}
\ No newline at end of file
diff -r 000000000000 -r 7ccd56e1c3b1 data_struct.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/data_struct.h Wed Aug 11 01:23:42 2010 +0000
@@ -0,0 +1,27 @@
+
+typedef struct _fcc_data {
+
+ unsigned short US; //2
+ unsigned short IR1; //2
+ unsigned short IR2; //2
+ unsigned short IR3; //2
+ unsigned short IR4; //2
+ unsigned short FCC_voltage; //2
+ char state_flag; //1
+ float roll, pitch, yaw; // 4, 4, 4
+ float g_roll, g_pitch, g_yaw; //4, 4, 4
+ float acc_x, acc_y, acc_z; //4, 4 ,4
+
+}FCC_DATA;
+FCC_DATA fcc;
+
+typedef struct _ahrs {
+
+ short packet; //2
+ unsigned char data_info; //1
+ float g_roll, g_pitch, g_yaw; //4, 4, 4
+ float acc_x, acc_y, acc_z; //4, 4 ,4
+ float roll, pitch, yaw; // 4, 4, 4
+
+}AHRS_DATA;
+AHRS_DATA IMU;
\ No newline at end of file
diff -r 000000000000 -r 7ccd56e1c3b1 i2C.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/i2C.h Wed Aug 11 01:23:42 2010 +0000
@@ -0,0 +1,18 @@
+I2C i2c(p9, p10); //sda, scl
+
+const int addr = 0x50; // define the I2C Address
+char bl_F[2], bl_B[2], bl_L[2], bl_R[2];
+
+void eyetosee(){
+
+ bl_F[1]= 0;
+ bl_B[1]= 0;
+ bl_R[1]= 0;
+ bl_L[1]= 9;
+
+ i2c.write(addr+2, bl_L, 2); // Send command string to Motor Left
+ i2c.write(addr+4, bl_B, 2); // Send command string to Motor Back
+ i2c.write(addr+6, bl_F, 2); // Send command string to Motor Front
+ i2c.write(addr+8, bl_R, 2); // Send command string to Motor Right
+
+}
\ No newline at end of file
diff -r 000000000000 -r 7ccd56e1c3b1 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Aug 11 01:23:42 2010 +0000
@@ -0,0 +1,27 @@
+#include "mbed.h"
+#include "Set_up.h"
+#include "data_struct.h"
+#include "ahrs.h"
+#include "to_gcs.h"
+#include "i2C.h"
+#include "rcrx.h"
+
+int main() {
+ setup();
+ SendData.attach(&toGCS, 0.02777);
+ Proccess.attach(&eyetosee, 0.1);
+ ahrs.attach(&ahrs_rec);
+ rc.rise(&PPM_rise); // attach the address of the PPM_rise function to the rising edge
+ rc.fall(&PPM_fall); // attach the address of the PPM_fall function to the falling edge
+ wait(0.5);
+
+ while(1) {
+ eyetosee();
+ myled = !myled;
+ for(char i=0; i<6; i++){
+ printf("%d\t", buf_ppm[i]);
+ if(i == 5) printf("\n");
+ wait(0.02);
+ }
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 7ccd56e1c3b1 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Aug 11 01:23:42 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da
diff -r 000000000000 -r 7ccd56e1c3b1 rcrx.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rcrx.h Wed Aug 11 01:23:42 2010 +0000
@@ -0,0 +1,18 @@
+Timer tick;
+InterruptIn rc(p11);
+short buf_ppm[5];
+char i = 0;
+
+void PPM_rise() {
+ tick.stop(); // Stop timer
+ buf_ppm[i]=tick.read_us(); // Read timer to buffer[i] i=turns
+ tick.reset(); // Reset timer
+ i++; // increment i.
+ if (i==6) i = 0;
+ return;
+}
+
+void PPM_fall() { // Rising edge on the serial comm
+ tick.start(); // Start the timer
+ return;
+}
\ No newline at end of file
diff -r 000000000000 -r 7ccd56e1c3b1 to_gcs.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/to_gcs.h Wed Aug 11 01:23:42 2010 +0000
@@ -0,0 +1,16 @@
+#define FCC_DATA_SIZE 52 //39 Direct from IMU
+
+void toGCS(){
+
+ fcc.US++;// = 11;
+ if(fcc.US>1000) fcc.US = 0;
+
+ char *FtoG = (char *)&fcc, i; //&IMU
+
+
+ radio.putc(0xFF);
+ radio.putc(0xFF);
+ for (i=0;i<FCC_DATA_SIZE;i++) radio.putc(*FtoG++);
+ radio.putc(0x0D);
+
+}
\ No newline at end of file