Diff: main.cpp

Revision:

1:88d73fd8099a

Parent:

0:f2830b957dea

Child:

2:6c6c5f2796fe

diff -r f2830b957dea -r 88d73fd8099a main.cpp
--- a/main.cpp	Thu Aug 17 02:23:07 2017 +0000
+++ b/main.cpp	Mon Sep 11 02:35:29 2017 +0000
@@ -1,127 +1,22 @@
+
 #include "mbed.h"
-
-/*
-    serialピン配置
-    {TX(motherマイコン側はRX),RX,VIN,GND}
-*/
-#define PI 3.14159265359
-#define dataNum 5
-#define SOLADDR 0xc1
-
-class Motor
-{
-    private:
-        I2C i2c;
-        char addr;
-    public:
-        void setAddr(int addr_){
-            addr = addr_;
-        }
-        void init(char addr,int freq = 100000){
-            setAddr(addr);
-            i2c.frequency(freq);
-            char d = 0;
-            i2c.write((addr+1) << 1,&d,1);
-            wait(0.01);
-        }
-        Motor(PinName sda,PinName scl,int addr):i2c(sda,scl){
-            init(addr);
-        }
-        Motor(I2C& i2c_,int addr):i2c(i2c_){
-            init(addr);
-        }
-        Motor& operator=(float fval) {
-            if(fabs(fval) < 0.1f)   free();
-            else{
-                if(fval < 0)        run(1,char(-1*fval*255));
-                else if(fval > 0)   run(0,char(fval*255));
-            }
-            return *this;
-        }
-        bool run(char mode,char speed){
-            bool f = 0;char d = speed;
-            if(mode == 0 || mode == 1)  f = i2c.write((addr+mode) << 1,&d,1);
-            wait(0.01);
-            return f;
-        }
-        bool stop(){
-            return run(1,0);
-        }
-        bool free(){
-            return run(0,0);
-    }
-};
-
-class omuni{
-    private:
-        I2C i2c;
-        Motor m1,m2,m3;
-        char RXData[dataNum];
-        int8_t map(int8_t in, int8_t inMin, int8_t inMax, int8_t outMin, int8_t outMax) {
-            // check it's within the range
-            if (inMin<inMax) {
-                if (in <= inMin)
-                    return outMin;
-                if (in >= inMax)
-                    return outMax;
-            } else { // cope with input range being backwards.
-                if (in >= inMin)
-                    return outMin;
-                if (in <= inMax)
-                    return outMax;
-            }
-            // calculate how far into the range we are
-            float scale = float(in-inMin)/float(inMax-inMin);
-            // calculate the output.
-            return int8_t(outMin + scale*float(outMax-outMin));
-        }
-    public:
-        omuni(I2C& i2c_,int8_t addr1,int8_t addr2,int8_t addr3)
-        :i2c(i2c_),m1(i2c_,addr1),m2(i2c_,addr2),m3(i2c_,addr3)
-        {
-            RXData[0] = 'H';
-            RXData[1] =  0 ;
-            RXData[2] =  0 ;
-            RXData[3] =  0 ;
-            RXData[4] =  0 ;
-        }
-        void out(char rxdata[dataNum]){
-            signed char x1 = map(rxdata[1],-90,90,-100,100);
-            signed char y1 = map(rxdata[2],-90,90,-100,100);
-            signed char x2 = map(rxdata[3],-90,90,-100,100);
-            signed char y2 = map(rxdata[4],-90,90,-100,100);
-            float r1 = pow(x1*x1+y1*y1,0.5)/168;
-            float r2 = pow(x2*x2+y2*y2,0.5)/168;
-            float sieta1 = atan2(double(x1),double(y1));
-            float sieta2 = atan2(double(x2),double(y2));
-            if(r2 < 0.1F)r2 = 0;
-            float alpha = PI/2;
-            float x_2 = cos(sieta2);
-            float y_2 = sin(sieta2);
-            m1 = float(sin(alpha)*x2*0.01);
-            m2 = float(sin(alpha+4.0/3*PI)*x2 - cos(alpha+4.0/3*PI)*y2)*0.01;
-            m3 = float(sin(alpha+2.0/3*PI)*x2 - cos(alpha+2.0/3*PI)*y2)*0.01;
-        }
-};
-
-
-
-//上は後にライブラリ化
+#include "omuni.h"
+#include "solenoid.h"
 
 I2C i2c(p28, p27);
 omuni omu(i2c, 0x10, 0x12, 0x14);
-
-int ConData[2][12];
-
-BusOut led(LED1,LED2,LED3,LED4);
+solenoid sol(i2c, 0x20);
 
 Serial con(p9, p10);
 Serial master(p13,p14);
 Serial pc(USBTX, USBRX);
 
+int ConData[2][12];
+
+
 void GetData()
 {
-    if ( con.getc() == 'H' ) { // ヘッダ文字を見つけたところから読み取る
+    if ( con.getc() == 'H' ) {
          ConData[0][0] = 'H';
          for (int i = 1; i < 12; i++)
          {
@@ -133,33 +28,40 @@
              char t = (char)con.getc();
              ConData[1][i] = t;
          }
+        for(int i = 0; i < 12; i++)
+        {
+                master.putc(ConData[0][i]);
+        }
+        for(int i = 0; i < 12; i++)
+        {
+                master.putc(ConData[1][i]);
+        }
     }
 }
 
-
 int main() {
         con.baud(115200);
         master.baud(115200);
         pc.baud(115200);
         con.attach(&GetData,Serial::RxIrq);
         
-        char soldata = 0;
-        
         while(1){
             
-            char MotorData[] = {'H', ConData[0][1]-1, ConData[0][2]-10, ConData[1][1]+1, ConData[1][2]-8};
+            char MotorData[] = {'H', ConData[0][1]-1, ConData[0][2]-6, ConData[1][1]+1, ConData[1][2]-8};
             omu.out(MotorData);
-            
+            /*
             for(int i = 0; i < 12; i++)
             {
-                    master.putc(ConData[0][i]);
+                    pc.printf("%3d ",ConData[0][i]);
             }
             for(int i = 0; i < 12; i++)
             {
-                    master.putc(ConData[1][i]);
+                    pc.printf("%3d ",ConData[1][i]);
             }
-            
-            soldata = (ConData[0][3] << 2) + ConData[1][3];
-            i2c.write(SOLADDR, &soldata, 1);
-        }   
+            pc.printf("\n\r");
+            */
+            char soldata = ((ConData[0][3] << 2) + ConData[1][3]) << 4d;
+            //printf("%d\n",soldata);
+            sol = soldata;
+        }  
 }