CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver1

robocup メイン

Dependencies:   AQM0802A L6470_lib PID mbed

main.cpp@2:054444aa1990, 2014-12-23 (annotated)

Committer:
ryuna
Date:
Tue Dec 23 08:08:24 2014 +0000
Revision:
2:054444aa1990
Parent:
1:7d4921b5d638 
???????; SetUp; StartLoop; IrReceive; LineReceive?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:ae08e2e1d82d 1 /***********************************
ryuna 0:ae08e2e1d82d 2 *RoboCupJunior Soccer B Open 2014
ryuna 0:ae08e2e1d82d 3 *Koshinestu progrum
ryuna 0:ae08e2e1d82d 4 *
ryuna 0:ae08e2e1d82d 5 *このプログラムでは以下の処理をする．
ryuna 0:ae08e2e1d82d 6 *　LPC1114FN28/102からI2Cを用いて各種センサデータを収集
ryuna 0:ae08e2e1d82d 7 *　
ryuna 0:ae08e2e1d82d 8 *　データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
ryuna 0:ae08e2e1d82d 9 *
ryuna 1:7d4921b5d638 10 *　MotorDriverにmaxonに命令
ryuna 0:ae08e2e1d82d 11 *　
ryuna 0:ae08e2e1d82d 12 *　SteppingMotorDriverにステアリング命令
ryuna 0:ae08e2e1d82d 13 *　
ryuna 0:ae08e2e1d82d 14 *　LCDでデバック
ryuna 0:ae08e2e1d82d 15 *
ryuna 0:ae08e2e1d82d 16 *　スイッチ4つとスタートスイッチで処理を実行
ryuna 1:7d4921b5d638 17 *
ryuna 1:7d4921b5d638 18 * キッカーのスイッチがどこかに入る
ryuna 0:ae08e2e1d82d 19 *
ryuna 0:ae08e2e1d82d 20 *************************
ryuna 0:ae08e2e1d82d 21 *
ryuna 0:ae08e2e1d82d 22 *Pin Map
ryuna 0:ae08e2e1d82d 23 *
ryuna 1:7d4921b5d638 24 * p5,p6,p7,p29,p30 >> SPI >>Stepping
ryuna 0:ae08e2e1d82d 25 *
ryuna 0:ae08e2e1d82d 26 * p9,p10 >> I2C orSerial >> LPC1114FN28/102 read
ryuna 0:ae08e2e1d82d 27 *
ryuna 0:ae08e2e1d82d 28 * p13,p14 >> Serial >> Motor
ryuna 0:ae08e2e1d82d 29 *
ryuna 0:ae08e2e1d82d 30 * p22~p26 >> DebugSw and StartSw
ryuna 0:ae08e2e1d82d 31 *
ryuna 0:ae08e2e1d82d 32 * p27,p28 >> I2C >> DebugLCD
ryuna 0:ae08e2e1d82d 33 *
ryuna 0:ae08e2e1d82d 34 *
ryuna 0:ae08e2e1d82d 35 *
ryuna 0:ae08e2e1d82d 36 ******************************/
ryuna 0:ae08e2e1d82d 37
ryuna 2:054444aa1990 38
ryuna 2:054444aa1990 39 /*
ryuna 2:054444aa1990 40
ryuna 2:054444aa1990 41 ****以下IRはNighは距離が近いものとする．
ryuna 2:054444aa1990 42
ryuna 2:054444aa1990 43 */
ryuna 2:054444aa1990 44
ryuna 2:054444aa1990 45
ryuna 2:054444aa1990 46
ryuna 2:054444aa1990 47
ryuna 2:054444aa1990 48
ryuna 2:054444aa1990 49
ryuna 0:ae08e2e1d82d 50 #include "mbed.h"
ryuna 0:ae08e2e1d82d 51 #include "L6470.h"
ryuna 0:ae08e2e1d82d 52 #include "PID.h"
ryuna 0:ae08e2e1d82d 53 #include "AQM0802A.h"
ryuna 0:ae08e2e1d82d 54 #include <math.h>
ryuna 0:ae08e2e1d82d 55 #include <sstream>
ryuna 0:ae08e2e1d82d 56
ryuna 2:054444aa1990 57 #define ADDRESS_R 0xA0
ryuna 2:054444aa1990 58 #define ADDRESS_L 0xC0
ryuna 2:054444aa1990 59
ryuna 2:054444aa1990 60 #define READ_IR 0x01
ryuna 2:054444aa1990 61
ryuna 1:7d4921b5d638 62 DigitalIn DebugSw[4] = {p22,p23,p24,p25};
ryuna 1:7d4921b5d638 63 DigitalIn StartSw(p26,PullUp);
ryuna 1:7d4921b5d638 64 DigitalOut Led[4] = {LED1,LED2,LED3,LED4};
ryuna 0:ae08e2e1d82d 65
ryuna 1:7d4921b5d638 66 I2C Sensor(p9,p10);//sda,scl
ryuna 1:7d4921b5d638 67 AQM0802A Lcd(p28,p27);//sda,scl
ryuna 1:7d4921b5d638 68 L6470 Step(p5,p6,p7,p29,p30);//mosi,miso,sck,#cs,busy(PullUp)
ryuna 1:7d4921b5d638 69 Serial Motor(p13,p14);//tx,rx
ryuna 1:7d4921b5d638 70 Serial pc(USBTX,USBRX);
ryuna 0:ae08e2e1d82d 71
ryuna 0:ae08e2e1d82d 72
ryuna 0:ae08e2e1d82d 73 extern string StringFIN;
ryuna 0:ae08e2e1d82d 74 extern void array(int,int,int,int);
ryuna 0:ae08e2e1d82d 75
ryuna 0:ae08e2e1d82d 76
ryuna 0:ae08e2e1d82d 77 int speed[4] = {0};
ryuna 0:ae08e2e1d82d 78
ryuna 0:ae08e2e1d82d 79
ryuna 2:054444aa1990 80 unsigned int IrReceive(uint8_t *far){
ryuna 2:054444aa1990 81 /*
ryuna 2:054444aa1990 82 Slave側はIRを要求した場合IRのみを返してくるとする．
ryuna 2:054444aa1990 83 irは値が大きいほうが近いと仮定する
ryuna 2:054444aa1990 84
ryuna 2:054444aa1990 85
ryuna 2:054444aa1990 86 */
ryuna 2:054444aa1990 87 char data_r[4],data_l[4];
ryuna 2:054444aa1990 88 bool val;
ryuna 2:054444aa1990 89 Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
ryuna 2:054444aa1990 90
ryuna 2:054444aa1990 91 val = Sensor.read(ADDRESS_R|1, data_r, 4);// PINGデータを受信
ryuna 2:054444aa1990 92 Led[0] = val;
ryuna 2:054444aa1990 93 wait_ms(5);
ryuna 2:054444aa1990 94 val = Sensor.read(ADDRESS_L|1, data_l, 4);
ryuna 2:054444aa1990 95 Led[0] = !val;
ryuna 2:054444aa1990 96
ryuna 2:054444aa1990 97 if((data_r[3]-data_l[3])>0)
ryuna 2:054444aa1990 98 *far = data_r[0]%36%6*6 + (data_l[0]%36%6+6);
ryuna 2:054444aa1990 99
ryuna 2:054444aa1990 100 if(data_r[2]>data_l[2]){
ryuna 2:054444aa1990 101 if(data_r[2]>data_l[1]){
ryuna 2:054444aa1990 102 return data_r[0]/36*12*12*12 + data_r[0]%36/6*12*12 + (data_l[0]/36+6)*12 + (data_l[0]%36/6+6);
ryuna 2:054444aa1990 103 }else{
ryuna 2:054444aa1990 104 return data_r[0]/36*12*12*12 +(data_l[0]/36+6)*12*12+ data_r[0]%36/6*12 +(data_l[0]%36/6+6);
ryuna 2:054444aa1990 105 }
ryuna 2:054444aa1990 106 }else{
ryuna 2:054444aa1990 107 if(data_l[2]>data_r[1]){
ryuna 2:054444aa1990 108 return (data_l[0]/36+6)*12*12*12 + (data_l[0]%36/6+6)*12*12 + data_r[0]/36*12 + data_r[0]%36/6;
ryuna 2:054444aa1990 109 }else{
ryuna 2:054444aa1990 110 return (data_l[0]/36+6)*12*12*12 +data_r[0]/36*12*12+ (data_l[0]%36/6+6)*12 +(data_r[0]%36/6+6);
ryuna 2:054444aa1990 111 }
ryuna 2:054444aa1990 112
ryuna 2:054444aa1990 113 }
ryuna 2:054444aa1990 114
ryuna 2:054444aa1990 115 }
ryuna 2:054444aa1990 116
ryuna 2:054444aa1990 117 unsigned int LineReceive(){
ryuna 2:054444aa1990 118 //先に要求しない場合はLineの状況を送信すること．
ryuna 2:054444aa1990 119 //順番適当
ryuna 2:054444aa1990 120 char data_r[2],data_l[2];
ryuna 2:054444aa1990 121 bool val;
ryuna 2:054444aa1990 122 //val = slave.read(address,data,length,repeat);
ryuna 2:054444aa1990 123 val = Sensor.read(ADDRESS_R|1, data_r, 1);
ryuna 2:054444aa1990 124 Led[1] = val;
ryuna 2:054444aa1990 125 wait_ms(5);
ryuna 2:054444aa1990 126 val = Sensor.read(ADDRESS_L|1, data_l, 1);
ryuna 2:054444aa1990 127 Led[1] = !val;
ryuna 2:054444aa1990 128
ryuna 2:054444aa1990 129 return data_r[1]<<8 + data_l[1];
ryuna 2:054444aa1990 130
ryuna 2:054444aa1990 131
ryuna 2:054444aa1990 132 }
ryuna 2:054444aa1990 133 void rotate(unsigned int times, bool dir){
ryuna 2:054444aa1990 134 /*
ryuna 2:054444aa1990 135 *回転速度，回転方向，回転角度等設定変更ののち回転．
ryuna 2:054444aa1990 136 *
ryuna 2:054444aa1990 137 *
ryuna 2:054444aa1990 138 *
ryuna 2:054444aa1990 139 */
ryuna 2:054444aa1990 140
ryuna 2:054444aa1990 141
ryuna 2:054444aa1990 142 }
ryuna 2:054444aa1990 143
ryuna 2:054444aa1990 144 void move(int vr, int vl){
ryuna 0:ae08e2e1d82d 145 double pwm[4] = {0};
ryuna 0:ae08e2e1d82d 146 uint8_t i = 0;
ryuna 2:054444aa1990 147 pwm[0] = vr/10.0;
ryuna 2:054444aa1990 148 pwm[1] = vl/10.0;
ryuna 2:054444aa1990 149 pwm[2] = 0;
ryuna 2:054444aa1990 150 pwm[3] = 0;
ryuna 0:ae08e2e1d82d 151
ryuna 0:ae08e2e1d82d 152 for(i = 0; i < 4; i++){
ryuna 0:ae08e2e1d82d 153 if(pwm[i] > 100){
ryuna 0:ae08e2e1d82d 154 pwm[i] = 100;
ryuna 0:ae08e2e1d82d 155 }else if(pwm[i] < -100){
ryuna 0:ae08e2e1d82d 156 pwm[i] = -100;
ryuna 0:ae08e2e1d82d 157 }
ryuna 0:ae08e2e1d82d 158 speed[i] = pwm[i];
ryuna 0:ae08e2e1d82d 159 }
ryuna 0:ae08e2e1d82d 160 }
ryuna 0:ae08e2e1d82d 161
ryuna 2:054444aa1990 162
ryuna 2:054444aa1990 163
ryuna 1:7d4921b5d638 164 //通信（モータ用）
ryuna 1:7d4921b5d638 165 void tx_motor(){
ryuna 1:7d4921b5d638 166 array(speed[0],speed[1],speed[3],speed[2]);
ryuna 1:7d4921b5d638 167 Motor.printf("%s",StringFIN.c_str());
ryuna 1:7d4921b5d638 168 }
ryuna 0:ae08e2e1d82d 169
ryuna 2:054444aa1990 170 void SetUp(){
ryuna 2:054444aa1990 171 /*初期化*/
ryuna 1:7d4921b5d638 172 Motor.baud(115200); //ボーレート設定
ryuna 1:7d4921b5d638 173 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
ryuna 1:7d4921b5d638 174 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
ryuna 2:054444aa1990 175 move(0,0);//停止
ryuna 2:054444aa1990 176
ryuna 2:054444aa1990 177 Step.Resets();
ryuna 2:054444aa1990 178 Step.ReleseSW(0,1);//記憶がない
ryuna 2:054444aa1990 179
ryuna 2:054444aa1990 180 Lcd.cls();
ryuna 2:054444aa1990 181
ryuna 2:054444aa1990 182 /*初期化終了*/
ryuna 0:ae08e2e1d82d 183
ryuna 0:ae08e2e1d82d 184
ryuna 2:054444aa1990 185 }
ryuna 2:054444aa1990 186 void StartLoop(){
ryuna 2:054444aa1990 187 /*
ryuna 2:054444aa1990 188 *スイッチが押されるまでロボットはスタートしない．
ryuna 2:054444aa1990 189 *
ryuna 2:054444aa1990 190 *待っている間にほかのスイッチが押された場合は押されている間その動作をする等.
ryuna 2:054444aa1990 191 *
ryuna 2:054444aa1990 192 *
ryuna 2:054444aa1990 193 */
ryuna 2:054444aa1990 194
ryuna 2:054444aa1990 195 }
ryuna 2:054444aa1990 196 int main() {
ryuna 2:054444aa1990 197
ryuna 2:054444aa1990 198 /*IrRanking*/
ryuna 2:054444aa1990 199 unsigned int IrNigh;//ranking min1 > min2 > min3 > min4
ryuna 2:054444aa1990 200 uint8_t IrFar;//far1 > far2;
ryuna 2:054444aa1990 201
ryuna 2:054444aa1990 202 /*Line*/
ryuna 2:054444aa1990 203 unsigned int Line;
ryuna 2:054444aa1990 204
ryuna 2:054444aa1990 205 /*Ping(仮)*/
ryuna 2:054444aa1990 206
ryuna 2:054444aa1990 207
ryuna 2:054444aa1990 208 /*move value*/
ryuna 2:054444aa1990 209 int vr,vl;
ryuna 2:054444aa1990 210
ryuna 2:054444aa1990 211 SetUp();
ryuna 2:054444aa1990 212
ryuna 2:054444aa1990 213 StartLoop();
ryuna 2:054444aa1990 214
ryuna 0:ae08e2e1d82d 215 while(1) {
ryuna 2:054444aa1990 216
ryuna 2:054444aa1990 217 IrNigh = IrReceive(&IrFar);//順位設定
ryuna 2:054444aa1990 218
ryuna 2:054444aa1990 219 /*ここにIRの場所によったプログラムを挿入*/
ryuna 2:054444aa1990 220
ryuna 2:054444aa1990 221 /*line*/
ryuna 2:054444aa1990 222 Line = LineReceive();
ryuna 2:054444aa1990 223
ryuna 2:054444aa1990 224
ryuna 2:054444aa1990 225
ryuna 2:054444aa1990 226 move(30,30);
ryuna 1:7d4921b5d638 227 wait(1.5);
ryuna 1:7d4921b5d638 228
ryuna 0:ae08e2e1d82d 229 }
ryuna 0:ae08e2e1d82d 230 }