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

main.cpp@0:f2830b957dea, 2017-08-17 (annotated)

Committer:
gaku_sigu
Date:
Thu Aug 17 02:23:07 2017 +0000
Revision:
0:f2830b957dea
Child:
1:88d73fd8099a
2017/8/17

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gaku_sigu 0:f2830b957dea 1 #include "mbed.h"
gaku_sigu 0:f2830b957dea 2
gaku_sigu 0:f2830b957dea 3 /*
gaku_sigu 0:f2830b957dea 4 serialピン配置
gaku_sigu 0:f2830b957dea 5 {TX(motherマイコン側はRX),RX,VIN,GND}
gaku_sigu 0:f2830b957dea 6 */
gaku_sigu 0:f2830b957dea 7 #define PI 3.14159265359
gaku_sigu 0:f2830b957dea 8 #define dataNum 5
gaku_sigu 0:f2830b957dea 9 #define SOLADDR 0xc1
gaku_sigu 0:f2830b957dea 10
gaku_sigu 0:f2830b957dea 11 class Motor
gaku_sigu 0:f2830b957dea 12 {
gaku_sigu 0:f2830b957dea 13 private:
gaku_sigu 0:f2830b957dea 14 I2C i2c;
gaku_sigu 0:f2830b957dea 15 char addr;
gaku_sigu 0:f2830b957dea 16 public:
gaku_sigu 0:f2830b957dea 17 void setAddr(int addr_){
gaku_sigu 0:f2830b957dea 18 addr = addr_;
gaku_sigu 0:f2830b957dea 19 }
gaku_sigu 0:f2830b957dea 20 void init(char addr,int freq = 100000){
gaku_sigu 0:f2830b957dea 21 setAddr(addr);
gaku_sigu 0:f2830b957dea 22 i2c.frequency(freq);
gaku_sigu 0:f2830b957dea 23 char d = 0;
gaku_sigu 0:f2830b957dea 24 i2c.write((addr+1) << 1,&d,1);
gaku_sigu 0:f2830b957dea 25 wait(0.01);
gaku_sigu 0:f2830b957dea 26 }
gaku_sigu 0:f2830b957dea 27 Motor(PinName sda,PinName scl,int addr):i2c(sda,scl){
gaku_sigu 0:f2830b957dea 28 init(addr);
gaku_sigu 0:f2830b957dea 29 }
gaku_sigu 0:f2830b957dea 30 Motor(I2C& i2c_,int addr):i2c(i2c_){
gaku_sigu 0:f2830b957dea 31 init(addr);
gaku_sigu 0:f2830b957dea 32 }
gaku_sigu 0:f2830b957dea 33 Motor& operator=(float fval) {
gaku_sigu 0:f2830b957dea 34 if(fabs(fval) < 0.1f) free();
gaku_sigu 0:f2830b957dea 35 else{
gaku_sigu 0:f2830b957dea 36 if(fval < 0) run(1,char(-1*fval*255));
gaku_sigu 0:f2830b957dea 37 else if(fval > 0) run(0,char(fval*255));
gaku_sigu 0:f2830b957dea 38 }
gaku_sigu 0:f2830b957dea 39 return *this;
gaku_sigu 0:f2830b957dea 40 }
gaku_sigu 0:f2830b957dea 41 bool run(char mode,char speed){
gaku_sigu 0:f2830b957dea 42 bool f = 0;char d = speed;
gaku_sigu 0:f2830b957dea 43 if(mode == 0 || mode == 1) f = i2c.write((addr+mode) << 1,&d,1);
gaku_sigu 0:f2830b957dea 44 wait(0.01);
gaku_sigu 0:f2830b957dea 45 return f;
gaku_sigu 0:f2830b957dea 46 }
gaku_sigu 0:f2830b957dea 47 bool stop(){
gaku_sigu 0:f2830b957dea 48 return run(1,0);
gaku_sigu 0:f2830b957dea 49 }
gaku_sigu 0:f2830b957dea 50 bool free(){
gaku_sigu 0:f2830b957dea 51 return run(0,0);
gaku_sigu 0:f2830b957dea 52 }
gaku_sigu 0:f2830b957dea 53 };
gaku_sigu 0:f2830b957dea 54
gaku_sigu 0:f2830b957dea 55 class omuni{
gaku_sigu 0:f2830b957dea 56 private:
gaku_sigu 0:f2830b957dea 57 I2C i2c;
gaku_sigu 0:f2830b957dea 58 Motor m1,m2,m3;
gaku_sigu 0:f2830b957dea 59 char RXData[dataNum];
gaku_sigu 0:f2830b957dea 60 int8_t map(int8_t in, int8_t inMin, int8_t inMax, int8_t outMin, int8_t outMax) {
gaku_sigu 0:f2830b957dea 61 // check it's within the range
gaku_sigu 0:f2830b957dea 62 if (inMin<inMax) {
gaku_sigu 0:f2830b957dea 63 if (in <= inMin)
gaku_sigu 0:f2830b957dea 64 return outMin;
gaku_sigu 0:f2830b957dea 65 if (in >= inMax)
gaku_sigu 0:f2830b957dea 66 return outMax;
gaku_sigu 0:f2830b957dea 67 } else { // cope with input range being backwards.
gaku_sigu 0:f2830b957dea 68 if (in >= inMin)
gaku_sigu 0:f2830b957dea 69 return outMin;
gaku_sigu 0:f2830b957dea 70 if (in <= inMax)
gaku_sigu 0:f2830b957dea 71 return outMax;
gaku_sigu 0:f2830b957dea 72 }
gaku_sigu 0:f2830b957dea 73 // calculate how far into the range we are
gaku_sigu 0:f2830b957dea 74 float scale = float(in-inMin)/float(inMax-inMin);
gaku_sigu 0:f2830b957dea 75 // calculate the output.
gaku_sigu 0:f2830b957dea 76 return int8_t(outMin + scale*float(outMax-outMin));
gaku_sigu 0:f2830b957dea 77 }
gaku_sigu 0:f2830b957dea 78 public:
gaku_sigu 0:f2830b957dea 79 omuni(I2C& i2c_,int8_t addr1,int8_t addr2,int8_t addr3)
gaku_sigu 0:f2830b957dea 80 :i2c(i2c_),m1(i2c_,addr1),m2(i2c_,addr2),m3(i2c_,addr3)
gaku_sigu 0:f2830b957dea 81 {
gaku_sigu 0:f2830b957dea 82 RXData[0] = 'H';
gaku_sigu 0:f2830b957dea 83 RXData[1] = 0 ;
gaku_sigu 0:f2830b957dea 84 RXData[2] = 0 ;
gaku_sigu 0:f2830b957dea 85 RXData[3] = 0 ;
gaku_sigu 0:f2830b957dea 86 RXData[4] = 0 ;
gaku_sigu 0:f2830b957dea 87 }
gaku_sigu 0:f2830b957dea 88 void out(char rxdata[dataNum]){
gaku_sigu 0:f2830b957dea 89 signed char x1 = map(rxdata[1],-90,90,-100,100);
gaku_sigu 0:f2830b957dea 90 signed char y1 = map(rxdata[2],-90,90,-100,100);
gaku_sigu 0:f2830b957dea 91 signed char x2 = map(rxdata[3],-90,90,-100,100);
gaku_sigu 0:f2830b957dea 92 signed char y2 = map(rxdata[4],-90,90,-100,100);
gaku_sigu 0:f2830b957dea 93 float r1 = pow(x1*x1+y1*y1,0.5)/168;
gaku_sigu 0:f2830b957dea 94 float r2 = pow(x2*x2+y2*y2,0.5)/168;
gaku_sigu 0:f2830b957dea 95 float sieta1 = atan2(double(x1),double(y1));
gaku_sigu 0:f2830b957dea 96 float sieta2 = atan2(double(x2),double(y2));
gaku_sigu 0:f2830b957dea 97 if(r2 < 0.1F)r2 = 0;
gaku_sigu 0:f2830b957dea 98 float alpha = PI/2;
gaku_sigu 0:f2830b957dea 99 float x_2 = cos(sieta2);
gaku_sigu 0:f2830b957dea 100 float y_2 = sin(sieta2);
gaku_sigu 0:f2830b957dea 101 m1 = float(sin(alpha)*x2*0.01);
gaku_sigu 0:f2830b957dea 102 m2 = float(sin(alpha+4.0/3*PI)*x2 - cos(alpha+4.0/3*PI)*y2)*0.01;
gaku_sigu 0:f2830b957dea 103 m3 = float(sin(alpha+2.0/3*PI)*x2 - cos(alpha+2.0/3*PI)*y2)*0.01;
gaku_sigu 0:f2830b957dea 104 }
gaku_sigu 0:f2830b957dea 105 };
gaku_sigu 0:f2830b957dea 106
gaku_sigu 0:f2830b957dea 107
gaku_sigu 0:f2830b957dea 108
gaku_sigu 0:f2830b957dea 109 //上は後にライブラリ化
gaku_sigu 0:f2830b957dea 110
gaku_sigu 0:f2830b957dea 111 I2C i2c(p28, p27);
gaku_sigu 0:f2830b957dea 112 omuni omu(i2c, 0x10, 0x12, 0x14);
gaku_sigu 0:f2830b957dea 113
gaku_sigu 0:f2830b957dea 114 int ConData[2][12];
gaku_sigu 0:f2830b957dea 115
gaku_sigu 0:f2830b957dea 116 BusOut led(LED1,LED2,LED3,LED4);
gaku_sigu 0:f2830b957dea 117
gaku_sigu 0:f2830b957dea 118 Serial con(p9, p10);
gaku_sigu 0:f2830b957dea 119 Serial master(p13,p14);
gaku_sigu 0:f2830b957dea 120 Serial pc(USBTX, USBRX);
gaku_sigu 0:f2830b957dea 121
gaku_sigu 0:f2830b957dea 122 void GetData()
gaku_sigu 0:f2830b957dea 123 {
gaku_sigu 0:f2830b957dea 124 if ( con.getc() == 'H' ) { // ヘッダ文字を見つけたところから読み取る
gaku_sigu 0:f2830b957dea 125 ConData[0][0] = 'H';
gaku_sigu 0:f2830b957dea 126 for (int i = 1; i < 12; i++)
gaku_sigu 0:f2830b957dea 127 {
gaku_sigu 0:f2830b957dea 128 char t = (char)con.getc();
gaku_sigu 0:f2830b957dea 129 ConData[0][i] = t;
gaku_sigu 0:f2830b957dea 130 }
gaku_sigu 0:f2830b957dea 131 for (int i = 0; i < 12; i++)
gaku_sigu 0:f2830b957dea 132 {
gaku_sigu 0:f2830b957dea 133 char t = (char)con.getc();
gaku_sigu 0:f2830b957dea 134 ConData[1][i] = t;
gaku_sigu 0:f2830b957dea 135 }
gaku_sigu 0:f2830b957dea 136 }
gaku_sigu 0:f2830b957dea 137 }
gaku_sigu 0:f2830b957dea 138
gaku_sigu 0:f2830b957dea 139
gaku_sigu 0:f2830b957dea 140 int main() {
gaku_sigu 0:f2830b957dea 141 con.baud(115200);
gaku_sigu 0:f2830b957dea 142 master.baud(115200);
gaku_sigu 0:f2830b957dea 143 pc.baud(115200);
gaku_sigu 0:f2830b957dea 144 con.attach(&GetData,Serial::RxIrq);
gaku_sigu 0:f2830b957dea 145
gaku_sigu 0:f2830b957dea 146 char soldata = 0;
gaku_sigu 0:f2830b957dea 147
gaku_sigu 0:f2830b957dea 148 while(1){
gaku_sigu 0:f2830b957dea 149
gaku_sigu 0:f2830b957dea 150 char MotorData[] = {'H', ConData[0][1]-1, ConData[0][2]-10, ConData[1][1]+1, ConData[1][2]-8};
gaku_sigu 0:f2830b957dea 151 omu.out(MotorData);
gaku_sigu 0:f2830b957dea 152
gaku_sigu 0:f2830b957dea 153 for(int i = 0; i < 12; i++)
gaku_sigu 0:f2830b957dea 154 {
gaku_sigu 0:f2830b957dea 155 master.putc(ConData[0][i]);
gaku_sigu 0:f2830b957dea 156 }
gaku_sigu 0:f2830b957dea 157 for(int i = 0; i < 12; i++)
gaku_sigu 0:f2830b957dea 158 {
gaku_sigu 0:f2830b957dea 159 master.putc(ConData[1][i]);
gaku_sigu 0:f2830b957dea 160 }
gaku_sigu 0:f2830b957dea 161
gaku_sigu 0:f2830b957dea 162 soldata = (ConData[0][3] << 2) + ConData[1][3];
gaku_sigu 0:f2830b957dea 163 i2c.write(SOLADDR, &soldata, 1);
gaku_sigu 0:f2830b957dea 164 }
gaku_sigu 0:f2830b957dea 165 }