Diff: main.cpp

Revision:

0:f2830b957dea

Child:

1:88d73fd8099a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Aug 17 02:23:07 2017 +0000
@@ -0,0 +1,165 @@
+#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;
+        }
+};
+
+
+
+//上は後にライブラリ化
+
+I2C i2c(p28, p27);
+omuni omu(i2c, 0x10, 0x12, 0x14);
+
+int ConData[2][12];
+
+BusOut led(LED1,LED2,LED3,LED4);
+
+Serial con(p9, p10);
+Serial master(p13,p14);
+Serial pc(USBTX, USBRX);
+
+void GetData()
+{
+    if ( con.getc() == 'H' ) { // ヘッダ文字を見つけたところから読み取る
+         ConData[0][0] = 'H';
+         for (int i = 1; i < 12; i++)
+         {
+             char t = (char)con.getc();
+             ConData[0][i] = t;
+         }
+         for (int i = 0; i < 12; i++)
+         {
+             char t = (char)con.getc();
+             ConData[1][i] = t;
+         }
+    }
+}
+
+
+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};
+            omu.out(MotorData);
+            
+            for(int i = 0; i < 12; i++)
+            {
+                    master.putc(ConData[0][i]);
+            }
+            for(int i = 0; i < 12; i++)
+            {
+                    master.putc(ConData[1][i]);
+            }
+            
+            soldata = (ConData[0][3] << 2) + ConData[1][3];
+            i2c.write(SOLADDR, &soldata, 1);
+        }   
+}