int main() {
printf("Online%\n\r");
wait_ms(500);
/*inturupts*/
w1_cha.fall(&count_w1_a);
w1_chb.fall(&count_w1_b);
w2_cha.fall(&count_w2_a);
w2_chb.fall(&count_w2_b);
c_forward(); //sets the robot to go forward
while(1) {
s1_check = s1;
s2_check = s2;
s3_check = s3;
//s4_check = s4;
if(s1_check != 1)
{
printf("S1 Hit \n\r");
}
if(s2_check != 1)
{
printf("S2 Hit \n\r");
}
if(s3_check != 1)
{
printf("S3 Hit \n\r");
}
// if(s4_check != 1)
//{
//printf("S4 Hit \n\r");
//}
if (w1_count_a != w1_dif_a){
w1_dif_a = w1_count_a;
printf("w1 count a = %d\n\r", w1_dif_a);
}
if (w1_count_b != w1_dif_b){
w1_dif_b = w1_count_b;
printf("w1 count b = %d\n\r", w1_dif_b);
}
if (w2_count_a != w2_dif_a){
w2_dif_a = w2_count_a;
printf("w2 count a = %d\n\r", w2_dif_a);
}
if (w2_count_b != w2_dif_b){
w2_dif_b = w2_count_b;
printf("w2 count b = %d\n\r", w2_dif_b);
}
/*goes backwards if it hits a detects a wall*/
if (s1 != 1 && s2 !=1)
{
backward();//to correct when aligning
c_backward();
}
/*goes forwards when detecting the wall*/
if (s3 != 1){
c_forward();
}
/*tries to aligin with the wall*/
if (s1 != 1 && s2 != 0)
{
while(s1 != 1 && s2 != 0){
c_right();
}
backward();
//dir = 0;
//c_backward();
}
/*tries to aligin with the wall*/
if (s1 != 0 && s2 != 1)
{
while(s1 != 0 && s2 != 1){
c_left();
}
backward();
//dir = 0;
//c_backward();
}
/*if all sensors triggured tries to turn to escape*/
if (s1 != 1 && s2 != 1 && s3 != 1)
{
turnl();
}
/*if no senors triggred tries to contiue on*/
if (s1 != 0 && s2 != 0 && s3 != 0)
{
if (dir == 0){
c_backward();
}
else{
c_forward();
}
}
}
}
void Player::Move()
{
static float MAX_SPEED = 1;
static float MINI_SPEED = 0.01;
static float ACCELATION = 0.05f;
static float ブレーキ = 0.8f;
//カメラの前方向
Vector3 c_front(matView._13,0,matView._33);
c_front.Normalize();
//カメラの右方向
Vector3 c_right(matView._11, 0, matView._31);
c_right.Normalize();
Vector3 temp = Vector3(0, 0, 0);
bool is_move = false;
if (KEY_Get(KEY_UP) && IsCanControl())
{
SetMotion(1);
temp += c_front;
is_move = true;
//if (velocity.Length() > MAX_SPEED)
//{
// velocity.Normalize();
// velocity *= MAX_SPEED;
//}
}
if (KEY_Get(KEY_DOWN) && IsCanControl())
{
SetMotion(1);
temp -= c_front;
is_move = true;
////if (velocity.Length() > MAX_SPEED)
////{
//// velocity.Normalize();
//// velocity *= MAX_SPEED;
////}
}
if (KEY_Get(KEY_RIGHT) && IsCanControl())
{
SetMotion(1);
temp += c_right;
is_move = true;
//if (velocity.Length() > MAX_SPEED)
//{
// velocity.Normalize();
// velocity *= MAX_SPEED;
//}
}
if (KEY_Get(KEY_LEFT) && IsCanControl())
{
is_move = true;
SetMotion(1);
temp -= c_right;
if (velocity.Length() > MAX_SPEED)
{
velocity.Normalize();
velocity *= MAX_SPEED;
}
}
temp.Normalize();
temp *= ACCELATION;
velocity += temp;
if (velocity.Length() > MAX_SPEED)
{
velocity.Normalize();
velocity *= MAX_SPEED;
}
if (!is_move)
{
velocity *= ブレーキ;
if (velocity.Length() < MINI_SPEED)
{
velocity.Normalize();
velocity =Vector3(0,0,0);
}
SetMotion(0);
}