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

Committer:
ryuna
Date:
2014-12-16
Revision:
1:7d4921b5d638
Parent:
0:ae08e2e1d82d
Child:
2:054444aa1990

File content as of revision 1:7d4921b5d638:

/***********************************
 *RoboCupJunior Soccer B Open 2014 
 *Koshinestu progrum
 *
 *このプログラムでは以下の処理をする．
 *　LPC1114FN28/102からI2Cを用いて各種センサデータを収集
 *　
 *　データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
 *
 *　MotorDriverにmaxonに命令
 *　
 *　SteppingMotorDriverにステアリング命令
 *　
 *　LCDでデバック
 *
 *　スイッチ4つとスタートスイッチで処理を実行
 *  
 * キッカーのスイッチがどこかに入る
 *
 ************************* 
 * 
 *Pin Map
 *
 *  p5,p6,p7,p29,p30 >> SPI >>Stepping
 *  
 *  p9,p10 >> I2C orSerial >> LPC1114FN28/102 read
 *
 *  p13,p14 >> Serial >> Motor
 *
 *  p22~p26 >> DebugSw and StartSw 
 *
 *  p27,p28 >> I2C  >> DebugLCD
 *
 *
 *
 ******************************/

#include "mbed.h"
#include "L6470.h"
#include "PID.h"
#include "AQM0802A.h"
#include <math.h>
#include <sstream>

DigitalIn   DebugSw[4] = {p22,p23,p24,p25};
DigitalIn   StartSw(p26,PullUp);
DigitalOut  Led[4] = {LED1,LED2,LED3,LED4};

I2C         Sensor(p9,p10);//sda,scl
AQM0802A    Lcd(p28,p27);//sda,scl
L6470       Step(p5,p6,p7,p29,p30);//mosi,miso,sck,#cs,busy(PullUp)
Serial      Motor(p13,p14);//tx,rx
Serial      pc(USBTX,USBRX);


extern string StringFIN;
extern void array(int,int,int,int);


int speed[4] = {0};            


void move(int vx, int vy, int vs, int vk){
    double pwm[4] = {0};
    uint8_t i = 0;
    pwm[0] = (double)((vx) + vs);
    pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0)  * vy) + vs);
    pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
    pwm[3] = vk;

    for(i = 0; i < 4; i++){
            if(pwm[i] > 100){
                pwm[i] = 100;
            }else if(pwm[i] < -100){
                pwm[i] = -100;
            }
        speed[i] = pwm[i];
    }
}

//通信（モータ用）
void tx_motor(){
    array(speed[0],speed[1],speed[3],speed[2]);
    Motor.printf("%s",StringFIN.c_str());
}

int main() {
    Motor.baud(115200);                             //ボーレート設定
    Motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
    Motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
    
    
    move(0,0,0,0);
    while(1) {
        move(30,30,0,0);
        wait(1.5);

    }
}