Mayu Wire Winding Setup With stepper motor ans Servos
Dependencies: BufferedSerial Servo X_NUCLEO_IHM01A1 mbed
Fork of WireWinding_Stepper by
main.cpp
- Committer:
- Arkadi
- Date:
- 2016-05-23
- Revision:
- 24:77e407b5d247
- Parent:
- 23:ef6c5c0d33b2
- Child:
- 25:7f8fd0790b13
File content as of revision 24:77e407b5d247:
/** ****************************************************************************** Wire Winding Setup Arkadiraf@gmail.com 23/5/2016 ****************************************************************************** */ /* Includes ------------------------------------------------------------------*/ /* mbed specific header files. */ #include "mbed.h" /* Helper header files. */ #include "DevSPI.h" /* Component specific header files. */ #include "l6474_class.h" #include "BufferedSerial.h" #include "Servo.h" Servo Servo1(D6); Servo Servo2(D3); /////////////// // variables // /////////////// BufferedSerial pc(USBTX, USBRX); char BufferCMD[64]={0}; uint16_t BufferIndex=0; int32_t CMDValue=0; // CMD at which speed to run char inbyte=' '; // variable to store stepper position int32_t StepperPos=0; int32_t StepperSetPos=0; /* Number of steps to move. */ #define STEPS2Rotation 3200 Ticker ServoCorrect; /* Variables -----------------------------------------------------------------*/ /* Initialization parameters. */ L6474_InitTypeDef init = { 1800, /* Acceleration rate in step/s2. Range: (0..+inf). */ 1800, /* Deceleration rate in step/s2. Range: (0..+inf). */ 1600, /* Maximum speed in step/s. Range: (30..10000]. */ 200, /* Minimum speed in step/s. Range: [30..10000). */ 400, /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */ L6474_OCD_TH_3000mA, /* Overcurrent threshold (OCD_TH register). */ L6474_CONFIG_OC_SD_ENABLE, /* Overcurrent shutwdown (OC_SD field of CONFIG register). */ L6474_CONFIG_EN_TQREG_TVAL_USED, /* Torque regulation method (EN_TQREG field of CONFIG register). */ L6474_STEP_SEL_1_4, /* Step selection (STEP_SEL field of STEP_MODE register). */ L6474_SYNC_SEL_1_2, /* Sync selection (SYNC_SEL field of STEP_MODE register). */ L6474_FAST_STEP_12us, /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */ L6474_TOFF_FAST_8us, /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */ 3, /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */ 21, /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */ L6474_CONFIG_TOFF_044us, /* Target Swicthing Period (field TOFF of CONFIG register). */ L6474_CONFIG_SR_320V_us, /* Slew rate (POW_SR field of CONFIG register). */ L6474_CONFIG_INT_16MHZ, /* Clock setting (OSC_CLK_SEL field of CONFIG register). */ L6474_ALARM_EN_OVERCURRENT | L6474_ALARM_EN_THERMAL_SHUTDOWN | L6474_ALARM_EN_THERMAL_WARNING | L6474_ALARM_EN_UNDERVOLTAGE | L6474_ALARM_EN_SW_TURN_ON | L6474_ALARM_EN_WRONG_NPERF_CMD /* Alarm (ALARM_EN register). */ }; /* Motor Control Component. */ L6474 *motor; /* Functions -----------------------------------------------------------------*/ /** * @brief This is an example of user handler for the flag interrupt. * @param None * @retval None * @note If needed, implement it, and then attach and enable it: * + motor->AttachFlagIRQ(&FlagIRQHandler); * + motor->EnableFlagIRQ(); * To disable it: * + motor->DisbleFlagIRQ(); */ void FlagIRQHandler(void) { /* Set ISR flag. */ motor->isr_flag = TRUE; /* Get the value of the status register. */ unsigned int status = motor->GetStatus(); /* Check NOTPERF_CMD flag: if set, the command received by SPI can't be performed. */ /* This often occures when a command is sent to the L6474 while it is not in HiZ state. */ if ((status & L6474_STATUS_NOTPERF_CMD) == L6474_STATUS_NOTPERF_CMD) pc.printf(" WARNING: \"FLAG\" interrupt triggered. Non-performable command detected when updating L6474's registers while not in HiZ state.\r\n"); /* Reset ISR flag. */ motor->isr_flag = FALSE; } void ServoUpdate(){ int32_t Rotations=motor->GetPosition()/STEPS2Rotation; int32_t amplitude=76; // number of rotation for full triangle wave 15/0.4/2*4 for 15 mm translation 0.4 wire diameter = float Y_New=(float) (abs(Rotations%amplitude-amplitude/2)-amplitude/4); float centr_Angle=0.30; // 0.55 minimum down , 0.05 minimum up. float Angle_Amplitude=0.25f; float Angle=centr_Angle+(Y_New/(amplitude/4))*Angle_Amplitude; //pc.printf("ServoUpdate %d, %d, %f \r\n",motor->GetPosition(),Rotations,Angle); // debug check/ Servo1=(Angle); Servo2=(1-Angle); } /* Main ----------------------------------------------------------------------*/ int main() { pc.baud(57600); /*----- Initialization. -----*/ /* Initializing SPI bus. */ DevSPI dev_spi(D11, D12, D13); /* Initializing Motor Control Component. */ motor = new L6474(D2, D8, D7, D9, D10, dev_spi); if (motor->Init(&init) != COMPONENT_OK) exit(EXIT_FAILURE); /* Attaching and enabling interrupt handlers. */ motor->AttachFlagIRQ(&FlagIRQHandler); motor->EnableFlagIRQ(); /*----- Changing motor setting. -----*/ /* Setting High Impedance State to update L6474's registers. */ motor->SoftHiZ(); /* Changing step mode. */ motor->SetStepMode(STEP_MODE_1_16); //STEP_MODE_1_16 STEP_MODE_FULL /* Increasing the torque regulation current to 400mA. */ motor->SetParameter(L6474_TVAL, 1500); /* Max speed to 2400 step/s. */ motor->SetMaxSpeed(6400); /* Min speed to 200 step/s. */ motor->SetMinSpeed(500); /* set accelerations */ motor->SetAcceleration(500); motor->SetDeceleration(500); /* Setting the current position to be the home position. */ motor->SetHome(); ServoCorrect.attach(&ServoUpdate,0.1f); /* Requesting to go to a specified position. */ StepperSetPos=600*STEPS2Rotation; motor->GoTo(StepperSetPos); motor->WaitWhileActive(); StepperPos = motor->GetPosition(); /* Infinite Loop. */ while (0) { // receive Motor Command while (pc.readable()) { inbyte=pc.getc(); pc.printf("%c" ,inbyte); // debug check/ BufferCMD[BufferIndex]=inbyte; BufferIndex++; // parse incoming message format: "$<value>\r\n" if (inbyte=='$'){ // start of message BufferIndex=0; // initialize to start of parser }else if(inbyte=='\r'){ // end of message CMDValue=atoi(BufferCMD); BufferIndex=0; pc.printf("CMD: %d \r\n" ,CMDValue); // debug check/ StepperSetPos=CMDValue*STEPS2Rotation; /* Update Stepper Commad */ }//end parser }//end serial /* Requesting to go to a specified position. */ motor->GoTo(StepperSetPos); motor->WaitWhileActive(); StepperPos = motor->GetPosition(); pc.printf("Rotations: %d \r\n", StepperPos/STEPS2Rotation); wait(1); } }