Revision 0:8215f0743d86, committed 2013-04-19

Comitter:

akudohune

Date:

Fri Apr 19 09:13:32 2013 +0000

Commit message:

to yusuke

Changed in this revision

diff -r 000000000000 -r 8215f0743d86 HMC6352.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Fri Apr 19 09:13:32 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/HMC6352/#83c0cb554099

diff -r 000000000000 -r 8215f0743d86 PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Fri Apr 19 09:13:32 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19

diff -r 000000000000 -r 8215f0743d86 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 19 09:13:32 2013 +0000
@@ -0,0 +1,490 @@
+#include <math.h>
+#include <sstream>
+#include "mbed.h"
+#include "HMC6352.h"
+#include "PID.h"
+#include "main.h"
+
+
+
+void PidUpdata()
+{    
+    inputPID = (((int)(compass.sample() - (standard * 10.0) + 5400.0) % 3600) / 10.0);        
+    
+    //pc.printf("%f\n",timer1.read());
+    pid.setProcessValue(inputPID);
+    //timer1.reset();
+    
+    compassPID = -(pid.compute());
+    //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
+    //pc.printf("%f\n",compassPID);
+}
+
+void move(int vxx, int vyy, int vss)
+{
+    double motVal[MOT_NUM] = {0};
+    
+    motVal[0] = (double)(((0.5 * vxx)  + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1);
+    motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT2);
+    motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3);
+    motVal[3] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT4);
+    
+    for(uint8_t i = 0 ; i < MOT_NUM ; i++){
+        if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
+        else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
+        speed[i] = motVal[i];
+    }
+    //pc.printf("%s",StringFIN.c_str());
+}
+
+/***********  Serial interrupt  ***********/
+
+void Tx_interrupt()
+{
+    array(speed[0],speed[1],speed[2],speed[3]);
+    driver.printf("%s",StringFIN.c_str());
+    //pc.printf("%s",StringFIN.c_str());
+    //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
+}
+/*
+void Rx_interrupt()
+{
+    if(driver.readable()){
+        //pc.printf("%d\n",driver.getc());
+    }
+}*/
+
+
+/***********  Serial interrupt end **********/
+
+
+void init()
+{
+    uint8_t initFlag = 0;
+    FILE *fp;
+    
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    
+    StartButton.mode(PullUp);
+    CalibEnterButton.mode(PullUp);
+    CalibExitButton.mode(PullUp);
+    EEPROMButton.mode(PullUp);
+    
+    driver.baud(BAUD_RATE);
+    wait_ms(MOTDRIVER_WAIT);
+    driver.printf("1F0002F0003F0004F000\r\n"); 
+    device2.printf("START");
+    
+    device2.attach(&dev_rx,Serial::RxIrq);
+    device2.attach(&dev_tx,Serial::TxIrq);
+    
+    led1 = ON;
+    
+    while(StartButton){
+        if(!CalibEnterButton){
+            led1 = OFF;
+            led2 = ON;
+            compass.setCalibrationMode(ENTER);
+            while(CalibExitButton);
+            compass.setCalibrationMode(EXIT);
+            led2 = OFF;
+            led3 = ON;
+         }
+         if(!EEPROMButton){
+            led3 = OFF;
+            led1 = OFF;
+            led4 = ON;
+            initFlag = 1;
+            fp = fopen("/local/out.txt", "r");
+            fscanf(fp, "%lf",&standard);
+            fclose(fp);
+         }
+    }
+    
+    if(!initFlag){
+        standard = compass.sample() / 10.0;
+        fp = fopen("/local/out.txt", "w");
+        fprintf(fp, "%f",standard);
+        fclose(fp);
+    }
+    
+    led1 = OFF;
+    led3 = OFF;
+    led4 = OFF;
+    
+    pid.setInputLimits(0.0, 360.0);
+    pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
+    pid.setBias(0.0);
+    pid.setMode(AUTO_MODE);
+    pid.setSetPoint(180.0);
+    
+    pidUpdata.attach(&PidUpdata, 0.06);
+    wait_ms(500);       //danger
+    driver.attach(&Tx_interrupt, Serial::TxIrq);
+    //driver.attach(&Rx_interrupt, Serial::RxIrq);
+    
+    //timer1.start();
+}
+
+int main()
+{
+    int vx=0,vy=0,vs=0;
+    uint8_t flag = 0;
+    uint8_t comp_flag6 = 0;
+    uint8_t comp_flag10 = 0;
+    
+    init();
+           
+    while(1){
+        //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
+        //wait(0.1);
+        //pc.printf("%d\t,%d\n",direction,Distance);
+        
+        //pc.printf("%d\n",distance);
+        
+        vx = 0;
+        vy = 0;
+        vs = compassPID;
+        
+        /*************** Change state **************************/
+        
+        if((Distance <= 30) && (IR_found) && (!flag)){
+            state = DIFFENCE;
+        }else{
+            if((direction == 4) || (direction == 6)|| (direction == 8) || (direction == 10)|| (direction == 12)){
+                state = DIFFENCE;       
+            }else if((Distance <= 120) && (IR_found) && (!flag)){
+                state = WARNING;
+            }else{
+                state = HOME_WAIT;
+            }
+        }
+        
+        if((IR_found) && (!flag)){
+            state = WARNING;
+        }else{
+            state = HOME_WAIT;
+        }
+            
+        /*************** Change state **************************/
+        
+        
+        if(state == HOME_WAIT){
+            comp_flag6 = 0;
+            comp_flag10 = 0;
+            if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 600.0)){
+                if((BACK_SONIC > 115.0) && (BACK_SONIC < 125.0)){
+                    vy = 0;
+                    flag = 0;
+                }else if(BACK_SONIC <= 115.0){
+                    vy = 3;
+                }else if(BACK_SONIC == PING_ERROR){
+                    vy = 5;
+                }else{
+                    vy = (int)(0.05 * (120.0 - BACK_SONIC) - 4);
+                    if(vy < -30)vy = -30;
+                }
+                
+                if((LEFT_SONIC > 450.0) && (RIGHT_SONIC > 450.0)){
+                    vx = 0;
+                }else if(RIGHT_SONIC < 450.0){
+                    vx = (int)((RIGHT_SONIC - 450.0) / 15.0 - 10.0);
+                    if(vx < -30)vx = -30;
+                }else if(LEFT_SONIC < 450.0){
+                    vx = (int)((450.0 - LEFT_SONIC ) / 15.0 + 10.0);
+                    if(vx > 30)vx = 30;
+                }
+            }else if((RIGHT_SONIC + LEFT_SONIC) <= 600.0){
+                if(BACK_SONIC < 100.0){
+                    if(RIGHT_SONIC > LEFT_SONIC){
+                        vx = 5;
+                        vy = 10;
+                    }else{
+                        vx = -5;
+                        vy = 10;
+                    }
+                }else if(BACK_SONIC < 200.0){
+                     if(RIGHT_SONIC > LEFT_SONIC){
+                        vx = 20;
+                        vy = 0;
+                    }else{
+                        vx = -20;
+                        vy = 0;
+                    }
+                }
+            }else{
+                if(BACK_SONIC > 500.0){
+                    if(RIGHT_SONIC > LEFT_SONIC){
+                        vx = 25;
+                        vy = -10;
+                    }else{                    
+                        vx = -25;
+                        vy = -10;
+                    }
+                }
+            }
+        }else if(state == DIFFENCE){
+            if(direction == 6){
+                comp_flag10 = 0;
+                if(comp_flag6){
+                    if(BACK_SONIC == PING_ERROR){
+                        
+                    }else if(BACK_SONIC > 50.0){
+                        vy = -10;
+                    }else{
+                        
+                    }
+                    
+                    if(LEFT_SONIC < 500.0){
+                        vy = -20;
+                    }else if(LEFT_SONIC == PING_ERROR){
+                        vx = 20;
+                    }else{
+                        vx = 20;
+                        vy = -20;
+                    }
+                }else{
+                    if(BACK_SONIC < 110.0){
+                        vy = 5;
+                    }else  if(BACK_SONIC > 500.0){
+                        vy = -20;
+                    }else if(BACK_SONIC > 300.0){
+                        vy = -10;
+                    }else if(BACK_SONIC > 140.0){
+                        vy = -5;
+                    }else{
+                        vy = 0;
+                    }
+                     
+                    if(LEFT_SONIC < 700.0){
+                        vx = 20;
+                    }else{
+                        if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
+                            comp_flag6 = 1;
+                        }else{
+                            vx = 5;
+                            vy = -15;
+                        }
+                    }
+                }
+            }else if(direction == 10){
+                comp_flag6 = 0;
+                if(comp_flag10){
+                    if(BACK_SONIC == PING_ERROR){
+                        
+                    }else if(BACK_SONIC > 50.0){
+                        vy = -10;
+                    }else{
+                        
+                    }
+                    
+                    if(RIGHT_SONIC < 500.0){
+                        vy = -20;
+                    }else if(RIGHT_SONIC == PING_ERROR){
+                        vx = -20;
+                    }else{
+                        vx = -20;
+                        vy = -20;
+                    }
+                }else{
+                
+                    if(BACK_SONIC < 110.0){
+                        vy = 5;
+                    }else  if(BACK_SONIC > 500.0){
+                        vy = -20;
+                    }else if(BACK_SONIC > 300.0){
+                        vy = -10;
+                    }else if(BACK_SONIC > 140.0){
+                        vy = -5;
+                    }else{
+                        vy = 0;
+                    }
+                     
+                    if(RIGHT_SONIC < 700.0){
+                        vx = -20;
+                    }else{
+                        if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
+                            comp_flag10 = 1;
+                        }else{
+                            vx = -5;
+                            vy = -15;
+                        }
+                    }
+                }  
+                
+            }else if(direction == 4){
+                comp_flag10 = 0;
+                if(comp_flag6){
+                    if(BACK_SONIC == PING_ERROR){
+                        
+                    }else if(BACK_SONIC > 50.0){
+                        vy = -10;
+                    }else{
+                        
+                    }
+                
+                    if(LEFT_SONIC < 500.0){
+                        vy = -20;
+                    }else if(LEFT_SONIC == PING_ERROR){
+                        vx = 20;
+                    }else{
+                        vx = 20;
+                        vy = -20;
+                    }
+                }else{
+                    if(BACK_SONIC < 110.0){
+                        vy = 5;
+                    }else  if(BACK_SONIC > 500.0){
+                        vy = -20;
+                    }else if(BACK_SONIC > 300.0){
+                        vy = -10;
+                    }else if(BACK_SONIC > 140.0){
+                        vy = -5;
+                    }else{
+                        vy = 0;
+                    }
+                     
+                    if(LEFT_SONIC < 700.0){
+                        vx = 20;
+                    }else{
+                        if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
+                            comp_flag6 = 1;
+                        }else{
+                            vx = 5;
+                            vy = -15;
+                        }
+                    }
+                }
+            }else if(direction == 12){
+                comp_flag6 = 0;
+                if(comp_flag10){
+                    if(BACK_SONIC == PING_ERROR){
+                        
+                    }else if(BACK_SONIC > 50.0){
+                        vy = -10;
+                    }else{
+                        
+                    }
+                    
+                    if(RIGHT_SONIC < 500.0){
+                        vy = -20;
+                    }else if(RIGHT_SONIC == PING_ERROR){
+                        vx = -20;
+                    }else{
+                        vx = -20;
+                        vy = -20;
+                    }
+                }else{
+                    if(BACK_SONIC < 110.0){
+                        vy = 5;
+                    }else  if(BACK_SONIC > 500.0){
+                        vy = -20;
+                    }else if(BACK_SONIC > 300.0){
+                        vy = -10;
+                    }else if(BACK_SONIC > 140.0){
+                        vy = -5;
+                    }else{
+                        vy = 0;
+                    }
+                     
+                    if(RIGHT_SONIC < 700.0){
+                        vx = -20;
+                    }else{
+                        if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
+                            comp_flag10 = 1;
+                        }else{
+                            vx = -5;
+                            vy = -15;
+                        }
+                    }
+                }
+            }else if(direction == 8){
+                if(comp_flag6){
+                    vx = -15;
+                    vy = -20;
+                }else if(comp_flag10){
+                    vx = 15;
+                    vy = -20;
+                }else{
+                    if(LEFT_SONIC > RIGHT_SONIC){
+                        vx = -15;
+                        vy = -20;
+                    }else{
+                        vx = 15;
+                        vy = -20;
+                    }
+                }
+            }else if((direction == 2) || (direction == 1)){
+                comp_flag6 = 0;
+                comp_flag10 = 0;
+                if((RIGHT_SONIC > 700.0) && (!(RIGHT_SONIC == PING_ERROR))){
+                    vx = 30;
+                }else{
+                    vx = 20;
+                }       
+                if(BACK_SONIC > 140.0){
+                    vy = -10;
+                }else if(BACK_SONIC < 110.0){
+                    vy = 5;
+                }
+            }else if((direction == 14) || (direction == 15)){
+                comp_flag6 = 0;
+                comp_flag10 = 0;
+                if((LEFT_SONIC > 700.0) && (!(LEFT_SONIC == PING_ERROR))){
+                    vx = -30;
+                }else{
+                    vx = -20;
+                }       
+                if(BACK_SONIC > 140.0){
+                    vy = -10;
+                }else if(BACK_SONIC < 110.0){
+                    vy = 5;
+                }
+            }else if(direction == 0){
+                comp_flag6 = 0;
+                comp_flag10 = 0;
+                vx = 0;
+                vy = 20;
+            }else{
+                comp_flag6 = 0;
+                comp_flag10 = 0;
+                vx = 0;
+                vy = 0;
+            }
+        }else if(state == WARNING){
+            comp_flag6 = 0;
+            comp_flag10 = 0;
+            if(direction == 0){
+                vx = 0;
+            }else if(direction == 1){
+                vx = 15;
+            }else if(direction == 2){
+                vx = 20;
+            }else if(direction == 14){
+                vx = -20;
+            }else if(direction == 15){
+                vx = -15;
+            }
+            
+            if((LEFT_SONIC > 320) && (RIGHT_SONIC > 320)){
+                if(BACK_SONIC > 150.0){
+                    vy = -5;
+                }else if(BACK_SONIC < 110.0){
+                    vy = 5;
+                }else{
+                    vy = 0;
+                }
+            }else{
+                if(BACK_SONIC > 110.0){
+                    vy = -5;
+                }else if(BACK_SONIC < 70.0){
+                    vy = 5;
+                }else{
+                    vy = 0;
+                }
+            }
+        }
+        move(vx,vy,vs);
+        
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r 8215f0743d86 main.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Fri Apr 19 09:13:32 2013 +0000
@@ -0,0 +1,76 @@
+
+
+#define RATE    0.052
+#define Long    1.0
+#define ENTER   0
+#define EXIT    1
+#define MOT_NUM 4
+#define MOTDRIVER_WAIT  300 //ms
+#define BAUD_RATE       115200
+#define ON      1
+#define OFF     0
+
+#define PING_ERROR  0xFFFF
+
+#define MOT1    1.0
+#define MOT2    1.0
+#define MOT3    1.0
+#define MOT4    1.0
+
+#define OUT_LIMIT   30.0
+#define MAX_POW     100
+#define MIN_POW     -100
+
+DigitalOut led1(LED1); 
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+//BusOut mbedleds(LED4,LED3,LED2,LED1); 
+HMC6352 compass(p9, p10);
+Serial driver(p28, p27); // tx, rx 
+Serial device2(p13, p14);  // tx, rx
+Serial pc(USBTX, USBRX); // tx, rx 
+DigitalIn StartButton(p21);
+DigitalIn CalibEnterButton(p22);
+DigitalIn CalibExitButton(p23);
+DigitalIn EEPROMButton(p24);
+PID pid(0.45, 1.0, 0.013, RATE);      //battery is low power version 30.0 0.58 1.0 0.015    battery is high power version   30.0 0.42, 1.0, 0.013
+Ticker pidUpdata;
+Timer timer1;
+LocalFileSystem local("local");  // マウントポイントを定義（ディレクトリパスになる）
+
+enum{
+    HOME_WAIT,
+    DIFFENCE,
+    WARNING
+};
+
+
+
+int speed[MOT_NUM] = {0};
+uint8_t state = HOME_WAIT;
+
+static float inputPID = 180.0;
+static double standard;
+static float compassPID = 0.0;
+
+
+extern string StringFIN;
+
+extern uint8_t direction;
+extern uint8_t Distance;
+extern uint8_t IR_found;
+
+extern void PidUpdata(void);
+extern void array(int,int,int,int);
+
+extern void dev_rx(void);
+extern void dev_tx(void);
+
+extern float ultrasonicVal[4];
+
+#define BACK_SONIC  ultrasonicVal[2]
+#define RIGHT_SONIC ultrasonicVal[1]
+#define LEFT_SONIC  ultrasonicVal[3]    
+
+

diff -r 000000000000 -r 8215f0743d86 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Apr 19 09:13:32 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/5e5da4a5990b
\ No newline at end of file

diff -r 000000000000 -r 8215f0743d86 uart1.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uart1.cpp	Fri Apr 19 09:13:32 2013 +0000
@@ -0,0 +1,66 @@
+
+#include "mbed.h"
+#include "uart1.h"
+#include "HMC6352.h"
+
+extern Serial device2;
+extern HMC6352 compass;
+
+extern uint8_t state;
+
+float ultrasonicVal[4];
+uint8_t direction;
+uint8_t Distance;
+uint8_t IR_found;
+uint8_t xbee;
+
+void dev_rx()
+{
+    static uint8_t count;
+    static uint8_t RecData[RECEIVE_DATA_NUM];
+    
+    RecData[count] = device2.getc();
+    
+    if(RecData[KEY] == KEYCODE){
+        count++;
+    }else{
+        count = 0;
+    }
+    if(count >= RECEIVE_DATA_NUM){
+        if(RecData[CHECK] == CHECKCODE){
+            //mbedleds = 1;
+            direction = RecData[DIRECTION];
+            Distance = RecData[DISTANCE];
+            ultrasonicVal[0] = (RecData[SONIC1_1] + (RecData[SONIC1_2] << 8)) / 10.0;
+            ultrasonicVal[1] = (RecData[SONIC2_1] + (RecData[SONIC2_2] << 8)) / 10.0;
+            ultrasonicVal[2] = (RecData[SONIC3_1] + (RecData[SONIC3_2] << 8)) / 10.0;
+            ultrasonicVal[3] = (RecData[SONIC4_1] + (RecData[SONIC4_2] << 8)) / 10.0;
+            xbee = RecData[XBEE];
+            IR_found = xbee;
+        }
+        count = 0;
+    }  
+}
+
+void dev_tx()
+{
+    static uint8_t count2;
+    static uint8_t SendData[SEND_DATA_NUM];
+    
+    //mbedleds = 2;
+    
+    if(count2 >= SEND_DATA_NUM){
+        SendData[KEY2]     = KEYCODE2;
+        SendData[DATA1]    = ((int)(compass.sample())) >> 8 ;
+        SendData[DATA2]    = (int)(compass.sample());
+        SendData[DATA3]    = state;
+        SendData[DATA4]    = 1;
+        SendData[CHECK2]   = CHECKCODE2;
+        
+        count2 = 0;
+        
+    }
+    device2.putc(SendData[count2]);
+    
+    count2++;
+}
\ No newline at end of file

diff -r 000000000000 -r 8215f0743d86 uart1.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uart1.h	Fri Apr 19 09:13:32 2013 +0000
@@ -0,0 +1,35 @@
+
+
+
+#define RECEIVE_DATA_NUM    13
+#define SEND_DATA_NUM       6
+
+#define KEYCODE             120
+#define CHECKCODE           (RecData[DIRECTION] ^ RecData[DISTANCE] ^ RecData[SONIC1_1] ^ RecData[SONIC1_2] ^ RecData[SONIC2_1] ^ RecData[SONIC2_2] ^ RecData[SONIC3_1] ^ RecData[SONIC3_2] ^ RecData[SONIC4_1] ^ RecData[SONIC4_2] ^ RecData[XBEE])
+#define KEYCODE2            35
+#define CHECKCODE2          (SendData[DATA1] ^ SendData[DATA2] ^ SendData[DATA3] ^ SendData[DATA4])
+
+
+enum{
+    KEY = 0,
+    DIRECTION,
+    DISTANCE,
+    SONIC1_1,
+    SONIC1_2,
+    SONIC2_1,
+    SONIC2_2,
+    SONIC3_1,
+    SONIC3_2,
+    SONIC4_1,
+    SONIC4_2,
+    XBEE,
+    CHECK,
+};
+enum{
+    KEY2 = 0,
+    DATA1,
+    DATA2,
+    DATA3,
+    DATA4,
+    CHECK2,
+};

diff -r 000000000000 -r 8215f0743d86 wordString.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wordString.cpp	Fri Apr 19 09:13:32 2013 +0000
@@ -0,0 +1,71 @@
+
+#include <sstream>
+#include "mbed.h"
+
+string StringFIN;
+
+using namespace std;
+
+
+extern Serial pc; // tx, rx 
+
+string IntToString(int number)
+{
+  stringstream ss;
+  ss << number;
+  return ss.str();
+}
+
+void array(int power1,int power2,int power3,int power4)
+{
+    int input[4] = {power1,power2,power3,power4};
+    int value = 0;
+    string StringA[4] = {"0","0","0","0"};
+    
+    
+    string StringX = "0";
+    string StringY = "0";
+    string StringZ = "0";
+    string String0 = "0";
+    
+    StringFIN = "0";
+    
+    for(uint8_t i = 0 ; i < 4; i++){
+        
+        value = input[i];
+        
+        StringX =  IntToString(i+1);
+        
+        if( (value < 0) && (value >= -100) ){
+            StringY = "R";
+            value = abs(value);
+            StringZ = IntToString(value);
+        }else if( (value >= 0) && (value <= 100) ){
+            StringY = "F";
+            StringZ = IntToString(value);
+        }else{
+            value = abs(value);
+            StringY = "F";
+            StringZ = "000";
+        }
+        
+        if(value < 10){
+            String0 = "00";
+            StringZ = String0 + StringZ;
+        }else if(value < 100)
+        {
+            String0 = "0";
+            StringZ = String0 + StringZ;
+        }else{
+            
+        }
+        
+        StringA[i] = (StringX + StringY + StringZ);
+        
+        if(i == 0)StringFIN  = StringA[i];
+        else StringFIN  += StringA[i];
+        
+    }
+    
+    StringFIN += "\r\n";   
+}
\ No newline at end of file