Kiko Ishimoto / Mbed 2 deprecated sotuken_mother_2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ds3_si mbed omuni solenoid

Fork of 2017_Robocon_mother by gaku takasawa

Diff: main.cpp

Revision:
6:f5f0b60cd380
Parent:
5:d5aa02f1acbb
Child:
7:c24e61f00404
--- a/main.cpp	Wed Oct 11 04:33:26 2017 +0000
+++ b/main.cpp	Fri Oct 13 10:22:35 2017 +0000
@@ -4,6 +4,8 @@
 
 //#define DEBUG
 
+#define CON_OFFSET 15
+
 I2C i2c(p28, p27);
 omuni omu(&i2c, 0x10, 0x14, 0x16);
 solenoid sol(&i2c, 0x20);
@@ -12,11 +14,14 @@
 Serial master(p13,p14);
 Serial pc(USBTX, USBRX);
 
+Timer serialtimer;
+
 char ConData[2][12];
-
+char offset[4];
 
 void GetData()
 {
+    static bool main_flag = 1 ;
     if ( con.getc() == 'H' ) {
          ConData[0][0] = 'H';
          for (int i = 1; i < 12; i++)
@@ -37,6 +42,21 @@
         {
                 master.putc(ConData[1][i]);
         }
+        if(main_flag)
+        {
+            offset[0] = ConData[0][1];
+            offset[1] = ConData[0][2];
+            offset[2] = ConData[1][1];
+            offset[3] = ConData[1][2];
+            main_flag = 0;
+        }
+        ConData[0][1] -= offset[0];
+        ConData[0][2] -= offset[1];
+        ConData[1][1] -= offset[2];
+        ConData[1][2] -= offset[3];
+        
+        if( (char)255 - CON_OFFSET < ConData[1][1] || ConData[1][1] < CON_OFFSET)
+            ConData[1][1] = 0;
     }
 }
 
@@ -48,7 +68,7 @@
         
         while(1){
             
-            char MotorData[] = {'H', ConData[0][1]-1, ConData[0][2]-6, ConData[1][1]+1, ConData[1][2]-8};
+            char MotorData[] = {'H', ConData[0][1], ConData[0][2], ConData[1][1], ConData[1][2]};
             omu.out(MotorData);
 
             char soldata = ((ConData[0][3] << 2) + ConData[1][3]) << 4;
@@ -65,6 +85,12 @@
             }
             pc.printf("\n\r");
             
+            for(int i = 0; i < 4; i++)
+            {
+                    pc.printf("%3d ",offset[i]);
+            }
+            printf("\n");
+            
             printf("%d\n",soldata);
             #endif