Diff: main.cpp

Revision:

1:f19466574b75

Parent:

0:7857b4c95c75

Child:

2:5871a792937b

--- a/main.cpp	Sat Jul 13 17:17:40 2013 +0000
+++ b/main.cpp	Sat Jul 20 04:33:14 2013 +0000
@@ -1,16 +1,18 @@
 #include "mbed.h"
 #include "RS405cb.h" 
 #include "MU2Class.h"
+#include "myCAN.h"
+#include "IDDATA.h"
 
 //for debug
     Serial pc(USBTX,USBRX);
     DigitalOut led2(LED2);
 //for communication
-    CAN can(p30, p29);
+    myCAN can(p30, p29);
     MU2Class MyMu2(p9,p10);
     DigitalOut MU2_permit(p11);
+    Ticker Send_Call;
 //for drive actuator
-    //PwmOut motor1(p21);
     RS405cb servo(p13,p14,p18);//TX,RX,PERMIT
 //for fusing PWM
     PwmOut nikuromu1(p22);
@@ -18,15 +20,13 @@
     PwmOut nikuromu3(p24);
 //for Flight confirmation
     DigitalIn flightpin(p26);
- 
-//for CAN
-    #define CAN_BAUD 4000000
-    int  id;
-    char smsg[8];
- 
+
+//for pc
+    #define PC_BAUD 9600
+    #define COMMUNICATION_RATE 0.3
+    void data_send(void);
+   
 //for PWM
-    #define M_PERIOD 0.01 
-    #define MOTOR_OFF motor1.pulsewidth(0.0);
     #define N_PERIOD 0.01
     #define NIKUROMU1_OFF nikuromu1.pulsewidth(0.0);
     #define NIKUROMU2_OFF nikuromu2.pulsewidth(0.0);
@@ -39,121 +39,151 @@
     void RHI_Scan(void);
     
 int main() {
+    //for pc
+        pc.baud(PC_BAUD);
+        Send_Call.attach(&data_send,COMMUNICATION_RATE);
+
     //correspondence off
         MU2_permit = 1;//off
         
     //PWM init
-        //motor1.period(M_PERIOD);
-        //MOTOR_OFF
         nikuromu1.period(N_PERIOD);
         NIKUROMU1_OFF
         nikuromu2.period(N_PERIOD);
         NIKUROMU2_OFF
         nikuromu3.period(N_PERIOD);
         NIKUROMU3_OFF
-        
-    //for CAN init 
-        CANMessage msg;
-        can.frequency(CAN_BAUD);
-        
+  
+    //Sequence
         
-    //for servo test
-        //led2 =1;
-        //RHI_Scan();
-        //servo.Rotate_Servo_Float_Test(3);
-        //servo.ID_CHANGE(1,3);
-        /*
+        while(flightpin ==1){}
+    
+        MU2_permit = 0;//on
+        servo.TORQUE_ON(1);
+        servo.TORQUE_ON(2);
+        servo.TORQUE_ON(3);
+        wait(0.3);
+    
+        MyMu2.send("housyutukennti\r\n");
+    
+        wait(10);
+    
+        MyMu2.send("tizyounituita\r\n");
+    
+    
+        NIKUROMU1_ON
+        wait(6.0);
+        NIKUROMU1_OFF
+    
+        MyMu2.send("tumehiraita\r\n");
+    
+        wait(10);
+    
         float theta;
         theta=-150.0;
         servo.TORQUE_ON(3);
         servo.Rotate_Servo_Float(3,theta);
         while(1) {
-            if(can.read(msg)) {
-                pc.printf("Message received: %d,%d\n\r", msg.data[0],msg.data[2]);
-                if(msg.data[2]!=1){
+                if(can.get_a_z()!=1){
                     servo.TORQUE_ON(3);
                     theta += 1.0;
                     servo.Rotate_Servo_Float(3,theta);
-                    //wait(0.02);
                 }
-                else{servo.TORQUE_ON(3);}
+                else{
+                    servo.TORQUE_ON(3);
+                    break;
+                }   
                 led2 = !led2;
-            } 
         }
-        */
         
-    //Sequence
-    
-    while(flightpin ==1){}
-    
-    MU2_permit = 0;//on
-    
-    wait(5);
-    
-    
-    id=1;
-    smsg[0]=1;
-    can.write(CANMessage(id,smsg)); 
-    
-    NIKUROMU1_ON
-    wait(6.0);
-    NIKUROMU1_OFF
-    
-    wait(10);
+        MyMu2.send("uemuitayo\r\n");
     
-    float theta;
-    theta=-150.0;
-    servo.TORQUE_ON(3);
-    servo.Rotate_Servo_Float(3,theta);
-    while(1) {
-        if(can.read(msg)) {
-            pc.printf("Message received:%d,%d,%d\n\r",msg.id, msg.data[0],msg.data[2]);
-            if(msg.data[2]!=1){
-                servo.TORQUE_ON(3);
-                theta += 1.0;
-                servo.Rotate_Servo_Float(3,theta);
-            }
-            else{
-                servo.TORQUE_ON(3);
-                break;
-            }
-            led2 = !led2;
-         } 
-    }
+        wait(5.0);
+        NIKUROMU2_ON
+        wait(6.0);
+        NIKUROMU2_OFF
+    
+        MyMu2.send("tobirahiraita\r\n");
     
-    
-    wait(5.0);
-    NIKUROMU2_ON
-    wait(6.0);
-    NIKUROMU2_OFF
+        wait(5.0);
+        NIKUROMU3_ON
+        wait(6.0);
+        NIKUROMU3_OFF
     
-    wait(5.0);
-    NIKUROMU3_ON
-    wait(6.0);
-    NIKUROMU3_OFF
+        MyMu2.send("anntenatennkai\r\n");
     
-    RHI_Scan();   
+        MyMu2.send("sukyannhazime\r\n");
     
-    //MyMu2.send("Hello.This is mbed. ");   
+        RHI_Scan();  
 }
 
 void RHI_Scan(void){
     servo.TORQUE_ON(1);
-    servo.TORQUE_ON(2);
+    servo.TORQUE_ON(3);
     
     while(1){
         for(int j=0;j<30;j++){
             for(int i=0;i<181;i++){
                 servo.Rotate_Servo_Float(1,j*1.0);
-                servo.Rotate_Servo_Float(2,90.0-i*1.0);
+                servo.Rotate_Servo_Float(3,90.0-i*1.0);
                 wait(0.01);
             }
             for(int i=0;i<181;i++){
                 servo.Rotate_Servo_Float(1,j*1.0);
-                servo.Rotate_Servo_Float(2,-90.0+i*1.0);
+                servo.Rotate_Servo_Float(3,-90.0+i*1.0);
                 wait(0.01);
             }
             wait(0.05);
         }
     }
 }
+
+void data_send(void){
+    pc.printf("%s,%s,%s,%d,%d\r\n",can.get_time(),can.get_latitude(),can.get_longitude(),can.get_NoS(),can.get_a_z());
+    
+    char mu2data[100];
+    mu2data[0] = *(can.get_time()+0);
+    mu2data[1] = *(can.get_time()+1);
+    mu2data[2] = *(can.get_time()+2);
+    mu2data[3] = *(can.get_time()+3);
+    mu2data[4] = *(can.get_time()+4);
+    mu2data[5] = *(can.get_time()+5);
+    mu2data[6] = *(can.get_time()+6);
+    mu2data[7] = *(can.get_time()+7);
+        
+    mu2data[8]=',';
+        
+    mu2data[9] = *(can.get_latitude()+0);
+    mu2data[10] = *(can.get_latitude()+1);
+    mu2data[11] = *(can.get_latitude()+2);
+    mu2data[12] = *(can.get_latitude()+3);
+    mu2data[13] = *(can.get_latitude()+4);
+    mu2data[14] = *(can.get_latitude()+5);
+    mu2data[15] = *(can.get_latitude()+6);
+    mu2data[16] = *(can.get_latitude()+7);
+    mu2data[17] = *(can.get_latitude()+8);
+        
+    mu2data[18]=',';
+        
+    mu2data[19] = *(can.get_longitude()+0);
+    mu2data[20] = *(can.get_longitude()+1);
+    mu2data[21] = *(can.get_longitude()+2);
+    mu2data[22] = *(can.get_longitude()+3);
+    mu2data[23] = *(can.get_longitude()+4);
+    mu2data[24] = *(can.get_longitude()+5);
+    mu2data[25] = *(can.get_longitude()+6);
+    mu2data[26] = *(can.get_longitude()+7);
+    mu2data[27] = *(can.get_longitude()+8);
+    mu2data[28] = *(can.get_longitude()+9);
+    
+    mu2data[29] = ',';
+    
+    mu2data[30] = ((can.get_NoS())/10)+0x30;
+    mu2data[31] = (can.get_NoS()%10)+0x30;
+        
+    mu2data[32]='\r';
+    mu2data[33]='\n';
+    mu2data[34]='\0';
+        
+    MyMu2.send(mu2data);
+}