CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_F

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:   AQM0802A HMC6352 L6470_lib PID mbed

Fork of CatPot_Main_ver1 by CatPot 2015-2016

main.cpp@1:7d4921b5d638, 2014-12-16 (annotated)

Committer:
ryuna
Date:
Tue Dec 16 09:13:08 2014 +0000
Revision:
1:7d4921b5d638
Parent:
0:ae08e2e1d82d
Child:
2:054444aa1990
update_?????????????????MultiSerial???

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 0:ae08e2e1d82d 38 #include "mbed.h"
ryuna 0:ae08e2e1d82d 39 #include "L6470.h"
ryuna 0:ae08e2e1d82d 40 #include "PID.h"
ryuna 0:ae08e2e1d82d 41 #include "AQM0802A.h"
ryuna 0:ae08e2e1d82d 42 #include <math.h>
ryuna 0:ae08e2e1d82d 43 #include <sstream>
ryuna 0:ae08e2e1d82d 44
ryuna 1:7d4921b5d638 45 DigitalIn DebugSw[4] = {p22,p23,p24,p25};
ryuna 1:7d4921b5d638 46 DigitalIn StartSw(p26,PullUp);
ryuna 1:7d4921b5d638 47 DigitalOut Led[4] = {LED1,LED2,LED3,LED4};
ryuna 0:ae08e2e1d82d 48
ryuna 1:7d4921b5d638 49 I2C Sensor(p9,p10);//sda,scl
ryuna 1:7d4921b5d638 50 AQM0802A Lcd(p28,p27);//sda,scl
ryuna 1:7d4921b5d638 51 L6470 Step(p5,p6,p7,p29,p30);//mosi,miso,sck,#cs,busy(PullUp)
ryuna 1:7d4921b5d638 52 Serial Motor(p13,p14);//tx,rx
ryuna 1:7d4921b5d638 53 Serial pc(USBTX,USBRX);
ryuna 0:ae08e2e1d82d 54
ryuna 0:ae08e2e1d82d 55
ryuna 0:ae08e2e1d82d 56 extern string StringFIN;
ryuna 0:ae08e2e1d82d 57 extern void array(int,int,int,int);
ryuna 0:ae08e2e1d82d 58
ryuna 0:ae08e2e1d82d 59
ryuna 0:ae08e2e1d82d 60 int speed[4] = {0};
ryuna 0:ae08e2e1d82d 61
ryuna 0:ae08e2e1d82d 62
ryuna 1:7d4921b5d638 63 void move(int vx, int vy, int vs, int vk){
ryuna 0:ae08e2e1d82d 64 double pwm[4] = {0};
ryuna 0:ae08e2e1d82d 65 uint8_t i = 0;
ryuna 0:ae08e2e1d82d 66 pwm[0] = (double)((vx) + vs);
ryuna 0:ae08e2e1d82d 67 pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
ryuna 0:ae08e2e1d82d 68 pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
ryuna 0:ae08e2e1d82d 69 pwm[3] = vk;
ryuna 0:ae08e2e1d82d 70
ryuna 0:ae08e2e1d82d 71 for(i = 0; i < 4; i++){
ryuna 0:ae08e2e1d82d 72 if(pwm[i] > 100){
ryuna 0:ae08e2e1d82d 73 pwm[i] = 100;
ryuna 0:ae08e2e1d82d 74 }else if(pwm[i] < -100){
ryuna 0:ae08e2e1d82d 75 pwm[i] = -100;
ryuna 0:ae08e2e1d82d 76 }
ryuna 0:ae08e2e1d82d 77 speed[i] = pwm[i];
ryuna 0:ae08e2e1d82d 78 }
ryuna 0:ae08e2e1d82d 79 }
ryuna 0:ae08e2e1d82d 80
ryuna 1:7d4921b5d638 81 //通信（モータ用）
ryuna 1:7d4921b5d638 82 void tx_motor(){
ryuna 1:7d4921b5d638 83 array(speed[0],speed[1],speed[3],speed[2]);
ryuna 1:7d4921b5d638 84 Motor.printf("%s",StringFIN.c_str());
ryuna 1:7d4921b5d638 85 }
ryuna 0:ae08e2e1d82d 86
ryuna 0:ae08e2e1d82d 87 int main() {
ryuna 1:7d4921b5d638 88 Motor.baud(115200); //ボーレート設定
ryuna 1:7d4921b5d638 89 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
ryuna 1:7d4921b5d638 90 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
ryuna 0:ae08e2e1d82d 91
ryuna 0:ae08e2e1d82d 92
ryuna 1:7d4921b5d638 93 move(0,0,0,0);
ryuna 0:ae08e2e1d82d 94 while(1) {
ryuna 1:7d4921b5d638 95 move(30,30,0,0);
ryuna 1:7d4921b5d638 96 wait(1.5);
ryuna 1:7d4921b5d638 97
ryuna 0:ae08e2e1d82d 98 }
ryuna 0:ae08e2e1d82d 99 }