/**
* MPU9250の初期化(MPU9250のジャイロ,加速度センサ)
*
* @param void
* @return void
*/
void initMPU9250 (void)
{
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU9250_W;
I2CMasterBuffer[1] = 0x6B; //Address start MPU9250
I2CMasterBuffer[2] = 0x00;
I2CEngine();
wait1msec(10);
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU9250_W;
I2CMasterBuffer[1] = 0x37; //Address auxiliary I2C
I2CMasterBuffer[2] = 0x02;
I2CEngine();
wait1msec(10);
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU9250_W;
I2CMasterBuffer[1] = 0x1A; //Address digital low pass filter
I2CMasterBuffer[2] = 0x02; //100Hz
I2CEngine();
wait1msec(10);
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU9250_W;
I2CMasterBuffer[1] = 0x1B; //range ang
I2CMasterBuffer[2] = 0x18; //2000deg/s
I2CEngine();
wait1msec(10);
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU9250_W;
I2CMasterBuffer[1] = 0x1C; //range acc
I2CMasterBuffer[2] = 0x18; //16g
I2CEngine();
wait1msec(10);
}
/**
* Asa[]に感度調整値を格納.
*
* @param void
* @return void
*/
void initAK8963 (void)
{
volatile uint32_t i;
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = AK8963_W;
I2CMasterBuffer[1] = 0x0a; //フューズROMアクセスモードに移行
I2CMasterBuffer[2] = 0x0f; //
I2CEngine();
wait1msec(10);
I2CWriteLength = 2;
I2CReadLength = 3;
I2CMasterBuffer[0] = AK8963_W;
I2CMasterBuffer[1] = 0x10; //Address 3byte (adjust MagXYZ)
I2CMasterBuffer[2] = AK8963_R;
I2CEngine();
for(i = 0;i < 3;i ++)
{
Asa[i] = I2CSlaveBuffer[i];
}
wait1msec(10);
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = AK8963_W;
I2CMasterBuffer[1] = 0x0a;
I2CMasterBuffer[2] = 0x00; //パワーダウンモードに変更
I2CEngine();
wait1msec(100);
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = AK8963_W;
I2CMasterBuffer[1] = 0x0a;
I2CMasterBuffer[2] = 0x10; //16bitの分解能に変更
I2CEngine();
wait1msec(10);
}
void whoAmI(void)
{
uint16_t i;
setSendDataEnable(0);
while(1)
{
myPrintfUSB("//////////////// \n");
I2CWriteLength = 2;
I2CReadLength = 1;
I2CMasterBuffer[0] = AK8963_W;
I2CMasterBuffer[1] = 0x00; //Address 3byte (adjust MagXYZ)
I2CMasterBuffer[2] = AK8963_R;
I2CEngine();
myPrintfUSB("Who Am I AK8963 : %d \n", I2CSlaveBuffer[0]);
for ( i = 0; i < BUFSIZE; i++ ) //clear I2CSlaveBuffer
{
I2CSlaveBuffer[i] = 0x00;
}
wait1msec(1000);
myPrintfUSB("//////////////// \n");
I2CWriteLength = 2;
I2CReadLength = 1;
I2CMasterBuffer[0] = MPU9250_W;
I2CMasterBuffer[1] = 0x75; //Address 3byte (adjust MagXYZ)
I2CMasterBuffer[2] = MPU9250_R;
I2CEngine();
myPrintfUSB("Who Am I MPU9250 : %d \n", I2CSlaveBuffer[0]);
for ( i = 0; i < BUFSIZE; i++ ) //clear I2CSlaveBuffer
{
I2CSlaveBuffer[i] = 0x00;
}
wait1msec(1000);
}
}
task main()
{
for (int i = 0; i < 127; i++)
{
wait1Msec(100);
motor[port1] = i;
motor[port10] = i;
}
wait1msec(1000);
}
void mode_BluetoothSetting(void)
{
turnLED(0);
myPrintfUART("############ Bluetooth setting mode ################\n");
wait1msec(300);
myPrintfUART("$$$"); //コマンドモードに入る
wait1msec(2000);
myPrintfUART("SM,0\r"); //動作モードをスレーブモードに変更
wait1msec(2000);
myPrintfUART("SU,115K\r");//ボーレートを115200bpsに変更
wait1msec(2000);
//myPrintfUART("SN,RT-BLUETOOTH-9AXIS\r");//デバイス名の変更
wait1msec(2000);
myPrintfUART("R,1\r"); //リブート
wait1msec(2000);
while(1)
{
turnLED(1);
//モード選択へ遷移
if( getSWcount() > 1000)
{
myPrintfUART("\t return mode select \n");
if(getSWcount() == 0) break;
}
}
}
task leftRightOne(){
clearLCDLine(0);
clearLCDLine(1);
//SensorValue[leftIEM] = 0;
//SensorValue[rightIEM] = 0;
//Clear LCD
//jerk to get intake down
//turn(830,RIGHT,40);
// turn right 90 degrees
//drive(.43* clickspermeters, FORWARD, 127);
//forward to the line that the bonus sack is on999/8
//turn(550,LEFT,40);
//swivel turn to pick up yellow sack
//driveTime(500,FORWARD,100);
//driveTime(500,BACKWARD,100);
driveTime(500,FORWARD,100);
driveTime(500,BACKWARD,100);
drive(.35 * clickspermeters, FORWARD,100);
wait1msec(100);
drive(.15 * clickspermeters, FORWARD, 100);
wait1Msec(500);
drive(.45 * clickspermeters, FORWARD, 100); //.50 to .45
//forward to yellow sack
//turn to trough
wait1Msec(300);
//armTime(1150,RAISE,127);
//raise arm
swivel(310, RIGHT, 80);
drive(.2 * clickspermeters, FORWARD, 127);
//drive forward to goal
drive(.28 * clickspermeters, FORWARD, 127); //.20 to .23 to .26 to .28
wait1Msec(900);
driveTime(700, BACKWARD, 67);
//Let sacks out5
turn(650, LEFT, 80); //550 to 650
armTime(1800,RAISE,127);
drive(.51 * clickspermeters, FORWARD, 60); //127 to 80 //.53 to .58 to .52 to .48
autoIntake(2050, -127);
wait1Msec(200);
driveTime(700, BACKWARD, 95);
//auton=false;
/*
if (auton == false){
turn(200,LEFT,60);
driveTime(2000, BACKWARD, 100);
}
*/
}
task main()
{
waitForStart();
motor[Arm] = 154;
wait1Msec(3200);
motor[Arm] = 0; //initial Arm raise
motor[Left] = 154;
motor[Right] = 154;
wait1Msec(900);
motor[Left] = 0;
motor[Right] = 0; //drive up to basket
motor[Arm] = 154;
wait1Msec(1200);
motor[Arm] = 0; //raise arm to drop block into basket
motor[Left] = -154;
motor[Right] = -154;
wait1Msec(700);
motor[Left] = 0;
motor[Right] = 0; //back up to prepare to lower arm
motor[Arm] = -154;
wait1Msec(3500);
motor[Arm] = 0; //lower arm
motor[Left] = 154;
motor[Right] = -154;
wait1Msec(450); //turn counterclockwise 45 degrees
motor[Left] = 154;
motor[Right] = 154;
wait1Msec(2300); //drive infront of ramp
motor[Left] = -154;
motor[Right] = 154;
wait1msec(1100); //turn clock wise 90 degrees to face ramp
motor[Left] = 154;
motor[Right] = 154;
wait1Msec(1700);
motor[Left] = 0;
motor[Right] = 0; //drive up ramp
}
void bringTubeBack(){
motor[back]=-25;
drive(-50);
while(nMotorEncoder[back]>-30);
motor[back]=0;
while(nMotorEncoder[back]<-20);
drive(50);
wait1Msec(300);
drive(0);
//wait1Msec(500);
//dump();
//wait1Msec(1000);
rotateRight(60,35);
drive(0);
wait1msec(100);
drive(-75);
wait1Msec(500);
drive(10);
wait1Msec(50);
drive(0);
grabState=0;
}