void ACCEL_GetValues(int16_t *x, int16_t *y, int16_t *z) { int16_t xmg, ymg, zmg; xmg = MMA1_GetXmg(); ymg = MMA1_GetYmg(); zmg = MMA1_GetZmg(); *x = xmg; *y = ymg; *z = zmg; }
static portTASK_FUNCTION(Remote, pvParameters) { int32_t valMotR = 0; int32_t valMotL = 0; for(;;) { #if PL_HAS_REMOTE && PL_IS_FRDM protocol42 txdata; if (analogOn) { GetXY(&x_anal,&y_anal); txdata.target = isROBOcop; txdata.type = anal_x; txdata.data = x_anal; sendData42(txdata); txdata.type = anal_y; txdata.data = y_anal; sendData42(txdata); } if (accelOn){ x_ac = MMA1_GetXmg(); y_ac = MMA1_GetYmg(); x_accel = scaleFromAccelToU8(x_ac); y_accel = scaleFromAccelToU8(y_ac); txdata.target = isROBOcop; txdata.type = accel_x; txdata.data = x_accel; sendData42(txdata); txdata.type = accel_y; txdata.data = y_accel; sendData42(txdata); } #endif #if PL_HAS_MOTOR if(valX > 0) { valMotR = valY-valX; valMotL = valY; } else if (valX < 0) { valMotR = valY; valMotL = valY+valX; } else { valMotR = valY; valMotL = valY; } if((valY < 3) && (valY > -3)) { valMotR = 0; valMotL = 0; } MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), valMotR); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT) , valMotL); #endif FRTOS1_vTaskDelay(20/TRG_TICKS_MS); } }
static void ACCEL_GetValues(int16_t *x, int16_t *y, int16_t *z) { *x = MMA1_GetXmg(); *y = MMA1_GetYmg(); *z = MMA1_GetZmg(); }
static void StateMachine_Run(void){ int i; int distance; // Für Acceleration Sensor int16_t x = MMA1_GetXmg(); WAIT1_Waitms(5); int16_t y = MMA1_GetYmg(); switch (state) { case START: //SHELL_SendString((unsigned char*)"INFO: No calibration data present.\r\n"); WAIT1_Waitms(4700); DRV_EnableDisable(1); DRV_SetSpeed(3000,3000); //WAIT1_Waitms(40); //DRV_SetSpeed(3000,3000); //state = ATTACK; //DRV_SetSpeed(-4200,4200); // MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), MOT_DIR_FORWARD); //MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), MOT_DIR_FORWARD); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 30); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 30); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 40); /*int x0; int x5; int x0_cal; int x5_cal; x0 = Get_Reflectance_Values(0); x0_cal = x0 / 15; x5 = Get_Reflectance_Values(5); x5_cal = x5 / 15; MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), x0_cal); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), x5_cal); */ //uint16_t* calib_values_pointer; //calib_values_pointer = &SensorTimeType; //calib_values_pointer = S1_GetVal(); state = DRIVE; break; case DRIVE: DRV_SetSpeed(-4200,4200); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 40); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 40); if((Get_Reflectance_Values(0) <= 100) || (Get_Reflectance_Values(5) <= 100)) // 0 = weiss / 1000 = schwarz { state = TURN; break; } distance = US_usToCentimeters(US_Measure_us(),22); if((distance <= 30) && (distance != 0)){ state = ATTACK; } /* if((x >= 2000) || (x <= 2000)) { state = STOP; } if((y >= 2000) || (y <= 2000)) { state = STOP; } */ break; case TURN: //SHELL_SendString((unsigned char*)"...stopping calibration.\r\n"); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -40); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -40); // ATTACK DRV_SetSpeed(-5000,-5000); WAIT1_Waitms(500); /* MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -40); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 40); */ DRV_SetSpeed(4200,-4200); WAIT1_Waitms(300); DRV_SetSpeed(5000,5000); WAIT1_Waitms(50); if((Get_Reflectance_Values(0) > 400) || (Get_Reflectance_Values(5) > 500)) { state = DRIVE; } break; case ATTACK: DRV_SetSpeed(5500,5500); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 80); //MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 80); if((Get_Reflectance_Values(0) <= 100) || (Get_Reflectance_Values(5) <= 100)) // 0 = weiss / 1000 = schwarz { state = TURN; } /* if((x >= 2000) || (x <= -2000)) { state = STOP; } if((y>= 2000) || (y <= -2000)) { state = STOP; } */ break; case STOP: DRV_SetSpeed(0,0); /*MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0); MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);*/ /* if((x >= -300) && (x <= 300)) { if((y >= -300) && (y <= 300)) { state = DRIVE; } } */ break; } /* whitch */ }