void Move_Motor( void *pvParameters ) {
MotorsInit();
tMotorMove* motorActions = (tMotorMove*) pvParameters;
unsigned char actionIndex = 0;
while( motorActions[actionIndex].duration != 0 ) {
MotorDir( LEFT_SIDE, motorActions[actionIndex].leftDirection );
MotorDir( RIGHT_SIDE, motorActions[actionIndex].rightDirection );
MotorSpeed( LEFT_SIDE, motorActions[actionIndex].leftSpeed);
MotorSpeed( RIGHT_SIDE, motorActions[actionIndex].rightSpeed);
MotorRun(LEFT_SIDE);
MotorRun(RIGHT_SIDE);
vTaskDelay( motorActions[actionIndex].duration );
actionIndex++;
}
MotorStop(LEFT_SIDE);
MotorStop(RIGHT_SIDE);
while (1) {
vTaskDelay( 500 );
}
}
void ControlInit(void) {
control.reset = 1;
control.status_bits = 0;
control.speeds.angular_speed = 0,
control.speeds.linear_speed = 0;
control.last_finished_id = 0;
control.max_acc = ACC_MAX;
control.max_spd = SPD_MAX;
control.rot_spd_ratio = RATIO_ROT_SPD_MAX;
MotorsInit();
RobotStateInit();
FifoInit();
PIDInit(&PID_left);
PIDInit(&PID_right);
PIDSet(&PID_left, LEFT_P, LEFT_I, LEFT_D, LEFT_BIAS);
PIDSet(&PID_right, RIGHT_P, RIGHT_I, RIGHT_D, RIGHT_BIAS);
}
int
main(void)
{
unsigned long ulPHYMR0;
tBoolean bButtonWasPressed = false;
tMotorState sMotorState=STATE_STOPPED;
tWaveHeader sWaveHeader;
unsigned long ulWaveIndex = 1;
int losL,losH;
ROM_SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_16MHZ);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_ETH);
ulPHYMR0 = ROM_EthernetPHYRead(ETH_BASE, PHY_MR0);
ROM_EthernetPHYWrite(ETH_BASE, PHY_MR0, ulPHYMR0 | PHY_MR0_PWRDN);
// Display96x16x1Init(true);
// Display96x16x1StringDraw("MOTOR", 29, 0);
// Display96x16x1StringDraw("DEMO", 31, 1);
LEDsInit();
LED_On(LED_1);
PushButtonsInit();
BumpSensorsInit();
MotorsInit();
ROM_SysTickPeriodSet(ROM_SysCtlClockGet() / 100);
ROM_SysTickEnable();
ROM_SysTickIntEnable();
SoundInit();
// WaveOpen((unsigned long *) sWaveClips[ulWaveIndex].pucWav, &sWaveHeader);
//mozliwe ze trzeba przed kazdym odtworzeniem ;/
// while(WaveOpen((unsigned long *)
// sWaveClips[ulWaveIndex].pucWav,
// &sWaveHeader) != WAVE_OK);//do zablokowania w razie bledu
for(;;)
{
tBoolean bButtonIsPressed;
tBoolean bBumperIsPressed[2];
bButtonIsPressed = !PushButtonGetDebounced((tButton)1);
bBumperIsPressed[0] = !BumpSensorGetDebounced((tBumper)0);
bBumperIsPressed[1] = !BumpSensorGetDebounced((tBumper)1);
switch(sMotorState)
{
case STATE_STOPPED:
{
if(bButtonIsPressed && !bButtonWasPressed)
{
sterowanie(0,50,50);
sMotorState = STATE_RUNNING;
}
break;
}
case STATE_RUNNING:
{
if(bButtonIsPressed && !bButtonWasPressed)
{
MotorStop((tSide)0);
MotorStop((tSide)1);
sMotorState = STATE_STOPPED;
}
else if(bBumperIsPressed[0])
{
MotorStop((tSide)0);
MotorStop((tSide)1);
// WavePlayStart(&sWaveHeader); mozliwe ze tu
losL =10+ g_ulTickCount % 20;//i dać los zamiast 15,mamy losowe skrecanie(10-30)
losH =40+ g_ulTickCount % 30;//i dać los zamiast 60,mamy losowe skrecanie(40-70)
sterowanie(1,losL,losH);
sMotorState = STATE_TYL;
}
else if(bBumperIsPressed[1]){
MotorStop((tSide)0);
MotorStop((tSide)1);
losL =10+ g_ulTickCount % 20;//i dać los zamiast 15,mamy losowe skrecanie(10-30)
losH =40+ g_ulTickCount % 30;//i dać los zamiast 60,mamy losowe skrecanie(40-70)
sterowanie(1,losH,losL);
// WavePlayStart(&sWaveHeader); mozliwe ze tu
sMotorState = STATE_TYL;
}
break;
}
case STATE_TYL:
{//cofanie tez moze byc losowe np losH+losL(50-100)+160=(210-260)
while(cofanie<250);
MotorStop((tSide)0);
MotorStop((tSide)1);
sterowanie(0,50,50);
sMotorState = STATE_RUNNING;
break;
}
default:
{
MotorStop((tSide)1);
MotorStop((tSide)0);
sMotorState = STATE_STOPPED;
break;
}
}
bButtonWasPressed = bButtonIsPressed;
}
}
int motor_init(){
MotorsInit();
MotorSpeed(LEFT_SIDE , 50<<8);
MotorSpeed(RIGHT_SIDE , 50<<8);
return 1;
}
int main(void)
{
/* Configure Oscillator to operate the device at 40MHz.
* Fosc= Fin*M/(N1*N2), Fcy=Fosc/4
* Fosc= 7.37M*40/(2*2)=80Mhz for 7.37M input clock */
//OSCCONbits.POST
//POST<1:0>: Oscillator Postscaler Selection bits
//11 = Oscillator postscaler divides clock by 64
//10 = Oscillator postscaler divides clock by 16
//01 = Oscillator postscaler divides clock by 4
//00 = Oscillator postscaler does not alter clock
// SegwayInit();
// BroadcastAppend(Mpu5060.MPU5060Frame);
// BroadcastAppend(Pid.PidFrame);
// BroadcastAppend(Kalman.KalmanFrame);
// BroadcastAppend(Motors.MotorsFrame);
//
// BluetoothInit();
ADCInit();
MotorsInit();
//unsigned int MicStepArray[MICSTEPS];
// float a = sin(3.1415926535897932384626433832795/2);
// a=a+a;
// int i;
// union
// {
// struct
// {
// unsigned int EnA;
// unsigned int EnB;
// } MicStepStruct;
// unsigned long int MicStep;
// } MicStepUnion;
// #define INP MicStepUnion.MicStepStruct
// #define MST MicStepUnion
// for(i = 0; i < MICSTEPS; i++)
// {
// float a, b;
// a = sin((float)i/(float)(MICSTEPS) * (PI/2));
// b = ((float)T2_PR2 * sin((float)i/(float)(MICSTEPS) * (PI/2)));
// INP.EnA = (unsigned int)((float)T2_PR2 * sin((float)i/(float)(MICSTEPS) * (PI/2)));
// INP.EnB = (unsigned int)((float)T2_PR2 * cos((float)i/(float)(MICSTEPS) * (PI/2)));
// MicStepArray[i] = MST.MicStep;
// }
while(1)
{
if(Tick_100 && !Tick_100_)
{
Tick_100_++;
}
else if(Tick_100 && Tick_100_)
{
Tick_100_--;
}
if(IFS0bits.ADIF == 1)
IFS0bits.ADIF = 0;
}
}
