void DoAuto(void){ int tmp = 0; int b = 0; int i = 0; tmp = RefPos(); if(!tmp){ MotorStop(); return; } MotorStart(); for(;i<20;i++){ b = borr_auto[i]; //if tmp = Nstep(b); if(!tmp){ MotorStop(); return; } tmp = DrillHole(); if(!tmp){ MotorStop(); return; } } MotorStop(); return; }
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 ); } }
int motor_STOP(){ char str[28]; evalbot->state=WAIT; MotorStop(LEFT_SIDE); MotorStop(RIGHT_SIDE); sprintf(str,"!POS:x:%4i y:%4i a:%4i", evalbot->x, evalbot->y, evalbot->angle); mess_setString(str,28); return 1; }
/** * @brief This function handles Memory Manage exception. * @param None * @retval None */ void MemManage_Handler(void) { /* Go to infinite loop when Memory Manage exception occurs */ while (1) { //出现错误时停转所有电机,防止意外伤人 MotorStop(); } }
/** * @brief This function handles Hard Fault exception. * @param None * @retval None */ void HardFault_Handler(void) { /* Go to infinite loop when Hard Fault exception occurs */ while (1) { //出现错误时停转所有电机,防止意外伤人 MotorStop(); } }
void DoAuto(void){ int i = 0; if(!RefPos()){ MotorStop(); return; } MotorStart(); while(pattern[i] != 0xFF){ if(!Nstep(pattern[i])){ MotorStop(); return; } if(!DrillHole()){ MotorStop(); return; } i++; } MotorStop(); return; }
static void sysMotorEventHandle(uint8_t id, MotorEvent_t event) { bool doNextStep = false; if(MOTOR_EVENT_STEP_OVER == event) { doNextStep = true; if(g_curStepInfo->flag == 1) { MProtoCtrlResult(MPROTO_RESULT_SENOR_ERROR); //´«¸ÐÆ÷δ´¥·¢ } } else if(MOTOR_EVENT_SENSOR_TRIGGERED == event) { if(g_curStepInfo != NULL && g_curStepInfo->id == id && g_curStepInfo->flag == 1) { doNextStep = true; } } if(doNextStep) { MotorStop(id); if(g_stepCount == 0) { g_sysStatus = SYS_STATUS_IDLE; MProtoCtrlResult(MPROTO_RESULT_SUCCESS); g_curStepInfo = NULL; } else { g_curStepInfo++; g_stepCount--; MotorStart(g_curStepInfo->id, g_curStepInfo->dir, g_curStepInfo->cycle); } } }
void DoAuto(){ int pattern[21] = {0,1,1,1,1,1,1,1,2,1,5,2,2,2,2,4,4,3,8,2,255}; int antalSteg; int subrutinRes; if(RefPos()){ int i=0; MotorStart(); while(1){ antalSteg = pattern[i]; i++; if(antalSteg == 255) break; subrutinRes = Nstep(antalSteg); if(subrutinRes == 0) break; subrutinRes = DrillHole(); if(subrutinRes == 0) break; } } MotorStop(); }
int _tmain(int argc, _TCHAR* argv[]) { /*------------------------Constants---------------------------------------------------------*/ CPipe motor("MotorData",1024); CPipe motionComplete("Stopped",1024); char* address1 = "0"; //corresponds to motor driver 1 on ST400 char* address2 = "3"; //motor driver 3 int slew_rate = 400; //This is the maximum step rate (range is 16 to 8500 steps/sec) int first_rate = 100; //This is the initial step rate (range is 16 to 8500 steps/sec) int accel = 1; //This is the acceleration rate (range is 0 to 255 steps/sec^2) int current = 300; //Motor winding current in mAs int port = 3; //Port number varies depending on the computer used. //Check Device Manager for proper port assignment. int RightWheelDone = 0; //Bit 7 set to 1 when motor movement is completed int LeftWheelDone = 0; int jogSpeed = 500; // Jog Speed (testing purposes only) /*------------------------------------------------------------------------------------------*/ //Attempt to open the bluetooth port to begin sending commands. while(PortOpen(port, 9600)!=1) { char* error = ::RMVGetErrorMessage(); printf("Port not opened. Error=%s\n",error); Sleep(1000); } printf("Port successfully opened\n"); //Initialize the stepper motors MotorStationsInit(); //Configure the motors to half-step mode if(MotorConfigureMode(address1,1)!=1 || MotorConfigureMode(address2,1)!=1) { //An error has occured. Display the RMV error message to determine the error. char* error = ::RMVGetErrorMessage(); printf("Configure error=%s\n",error); } //Initialize the slew rate, first rate and acceleration if(MotorSpeed( address1, slew_rate, first_rate, accel )!=1 || MotorSpeed( address2, slew_rate, first_rate, accel )!=1) { //An error has occured. Display the RMV error message to determine the error. char* error = ::RMVGetErrorMessage(); printf("Motor Speed error=%s\n",error); } if ( MotorSetCurrent(address1, current )!=1 || MotorSetCurrent(address2,current)!=1) { //An error has occured. Display the RMV error message to determine the error. char* error = ::RMVGetErrorMessage(); printf("Set Current error = %s\n",error); } else { printf("\nMotor Current set to: %d",current); } //Main motor control loop. Read step instructions sent by Thursday and perform them. while(1){ //Await instructions from Trajectory Planning program (Thursday). motor.Read(&ST400,sizeof(ST400)); cout << "Steps (right, left) = "<< ST400.rightWheel << "\t"<<ST400.leftWheel << endl; //Set the number of steps the motors must travel. if(MotorNumberStepRel( address1 , -ST400.leftWheel)!=1 || MotorNumberStepRel( address2 , -ST400.rightWheel)!=1) { //An error has occured. Display the RMV error message to determine the error. char* error = ::RMVGetErrorMessage(); printf("Step setting error=%s\n",error); } //Put motors in jog mode (testing purposes only) Comment out MotorNumberStepRel if using. /*MotorJog ( address1, 1 ); MotorJog ( address2, 1 ); MotorChangeJogSpeed ( address1, jogSpeed); MotorChangeJogSpeed ( address2, jogSpeed));*/ //Tell the motors to start executing the received command if(MotorTriggerON(address1)!=1 || MotorTriggerON(address2)!=1) { //An error has occured. Display the RMV error message to determine the error. char* error = ::RMVGetErrorMessage(); printf("Trigger on error=%s\n",error); } //Loop until current command is completed (bit 7 is set to 1) or an obstacle is detected while(((RightWheelDone & 0x80) != 128) || ((LeftWheelDone & 0x80) != 128)){ //Read motor status register to determine if current motion has ended if(MotorGetStatusRegister(address1,&LeftWheelDone)!= 1 || MotorGetStatusRegister(address2,&RightWheelDone)!=1) { //An error has occured. Display the RMV error message to determine the error. char* error = ::RMVGetErrorMessage(); printf("Get Status error=%s\n",error); } //Check if Thursday has sent data and read it if(motor.TestForData()>=sizeof(ST400)) { motor.Read(&ST400,sizeof(ST400)); } //Check if Thursday has indicated motor motion should stop (i.e. if an obstacle has been //detected by the IR sensors) and end the current motor motion if it has. if(ST400.stop != 0) { //Stop the current motion if(MotorStop(address1)!=1 || MotorStop(address2)!=1) { //An error has occured. Display the RMV error message to determine the error. char* error = ::RMVGetErrorMessage(); printf("Stop error=%s\n",error); } ST400.stop = 0; //Exit nested while loop break; } } //Send a 1 to Thursday to indicate current instructions are completed. int done=1; motionComplete.Write(&done,sizeof(done)); //Reset done variables LeftWheelDone = 0; RightWheelDone = 0; MotorTriggerOFF(address1); MotorTriggerOFF(address2); /* if(MotorPowerOFF(address1)!=1 || MotorPowerOFF(address2)!=1) { //An error has occured. Display the RMV error message to determine the error. char* error = ::RMVGetErrorMessage(); printf("error=%s\n",error); }*/ } }
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; } }