Mayu_Water / Mbed 2 deprecated Mayu_WireWinding_Stepper

Mayu Wire Winding Setup With stepper motor ans Servos

Dependencies:   BufferedSerial Servo X_NUCLEO_IHM01A1 mbed

Fork of WireWinding_Stepper by Shock_Team

main.cpp@24:77e407b5d247, 2016-05-23 (annotated)

Committer:
Arkadi
Date:
Mon May 23 13:52:57 2016 +0000
Revision:
24:77e407b5d247
Parent:
23:ef6c5c0d33b2
Child:
25:7f8fd0790b13
Combination servo and stepper, stand alone mode with 600 rotation after power up.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Davidroid 0:e6a49a092e2a 1 /**
Davidroid 0:e6a49a092e2a 2 ******************************************************************************
Arkadi 24:77e407b5d247 3 Wire Winding Setup Arkadiraf@gmail.com 23/5/2016
Davidroid 0:e6a49a092e2a 4 ******************************************************************************
Davidroid 0:e6a49a092e2a 5 */
Davidroid 0:e6a49a092e2a 6
Davidroid 0:e6a49a092e2a 7
Davidroid 0:e6a49a092e2a 8 /* Includes ------------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 9
Davidroid 0:e6a49a092e2a 10 /* mbed specific header files. */
Davidroid 0:e6a49a092e2a 11 #include "mbed.h"
Davidroid 0:e6a49a092e2a 12
Davidroid 0:e6a49a092e2a 13 /* Helper header files. */
Davidroid 0:e6a49a092e2a 14 #include "DevSPI.h"
Davidroid 0:e6a49a092e2a 15
Davidroid 0:e6a49a092e2a 16 /* Component specific header files. */
Davidroid 0:e6a49a092e2a 17 #include "l6474_class.h"
Davidroid 0:e6a49a092e2a 18
Arkadi 23:ef6c5c0d33b2 19 #include "BufferedSerial.h"
Davidroid 0:e6a49a092e2a 20
Arkadi 24:77e407b5d247 21
Arkadi 24:77e407b5d247 22 #include "Servo.h"
Arkadi 24:77e407b5d247 23
Arkadi 24:77e407b5d247 24 Servo Servo1(D6);
Arkadi 24:77e407b5d247 25 Servo Servo2(D3);
Arkadi 24:77e407b5d247 26
Arkadi 23:ef6c5c0d33b2 27 ///////////////
Arkadi 23:ef6c5c0d33b2 28 // variables //
Arkadi 23:ef6c5c0d33b2 29 ///////////////
Arkadi 23:ef6c5c0d33b2 30 BufferedSerial pc(USBTX, USBRX);
Davidroid 0:e6a49a092e2a 31
Arkadi 23:ef6c5c0d33b2 32 char BufferCMD[64]={0};
Arkadi 23:ef6c5c0d33b2 33 uint16_t BufferIndex=0;
Arkadi 23:ef6c5c0d33b2 34 int32_t CMDValue=0; // CMD at which speed to run
Arkadi 23:ef6c5c0d33b2 35 char inbyte=' ';
Arkadi 23:ef6c5c0d33b2 36 // variable to store stepper position
Arkadi 23:ef6c5c0d33b2 37 int32_t StepperPos=0;
Arkadi 23:ef6c5c0d33b2 38 int32_t StepperSetPos=0;
Davidroid 10:8f031470ca9f 39 /* Number of steps to move. */
Arkadi 23:ef6c5c0d33b2 40 #define STEPS2Rotation 3200
Davidroid 0:e6a49a092e2a 41
Davidroid 0:e6a49a092e2a 42
Arkadi 24:77e407b5d247 43 Ticker ServoCorrect;
Davidroid 0:e6a49a092e2a 44 /* Variables -----------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 45
Davidroid 19:1cd7f65c155c 46 /* Initialization parameters. */
Davidroid 19:1cd7f65c155c 47 L6474_InitTypeDef init =
Davidroid 19:1cd7f65c155c 48 {
Arkadi 23:ef6c5c0d33b2 49 1800, /* Acceleration rate in step/s2. Range: (0..+inf). */
Arkadi 23:ef6c5c0d33b2 50 1800, /* Deceleration rate in step/s2. Range: (0..+inf). */
Davidroid 19:1cd7f65c155c 51 1600, /* Maximum speed in step/s. Range: (30..10000]. */
Arkadi 23:ef6c5c0d33b2 52 200, /* Minimum speed in step/s. Range: [30..10000). */
Arkadi 23:ef6c5c0d33b2 53 400, /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
Arkadi 24:77e407b5d247 54 L6474_OCD_TH_3000mA, /* Overcurrent threshold (OCD_TH register). */
Davidroid 19:1cd7f65c155c 55 L6474_CONFIG_OC_SD_ENABLE, /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
Davidroid 19:1cd7f65c155c 56 L6474_CONFIG_EN_TQREG_TVAL_USED, /* Torque regulation method (EN_TQREG field of CONFIG register). */
Davidroid 19:1cd7f65c155c 57 L6474_STEP_SEL_1_4, /* Step selection (STEP_SEL field of STEP_MODE register). */
Davidroid 19:1cd7f65c155c 58 L6474_SYNC_SEL_1_2, /* Sync selection (SYNC_SEL field of STEP_MODE register). */
Davidroid 19:1cd7f65c155c 59 L6474_FAST_STEP_12us, /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
Davidroid 19:1cd7f65c155c 60 L6474_TOFF_FAST_8us, /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
Davidroid 19:1cd7f65c155c 61 3, /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
Davidroid 19:1cd7f65c155c 62 21, /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
Davidroid 19:1cd7f65c155c 63 L6474_CONFIG_TOFF_044us, /* Target Swicthing Period (field TOFF of CONFIG register). */
Davidroid 19:1cd7f65c155c 64 L6474_CONFIG_SR_320V_us, /* Slew rate (POW_SR field of CONFIG register). */
Davidroid 19:1cd7f65c155c 65 L6474_CONFIG_INT_16MHZ, /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
Davidroid 19:1cd7f65c155c 66 L6474_ALARM_EN_OVERCURRENT |
Davidroid 19:1cd7f65c155c 67 L6474_ALARM_EN_THERMAL_SHUTDOWN |
Davidroid 19:1cd7f65c155c 68 L6474_ALARM_EN_THERMAL_WARNING |
Davidroid 19:1cd7f65c155c 69 L6474_ALARM_EN_UNDERVOLTAGE |
Davidroid 19:1cd7f65c155c 70 L6474_ALARM_EN_SW_TURN_ON |
Davidroid 19:1cd7f65c155c 71 L6474_ALARM_EN_WRONG_NPERF_CMD /* Alarm (ALARM_EN register). */
Davidroid 19:1cd7f65c155c 72 };
Davidroid 19:1cd7f65c155c 73
Davidroid 0:e6a49a092e2a 74 /* Motor Control Component. */
Davidroid 1:fbf28f3367aa 75 L6474 *motor;
Davidroid 0:e6a49a092e2a 76
Davidroid 0:e6a49a092e2a 77
Davidroid 19:1cd7f65c155c 78 /* Functions -----------------------------------------------------------------*/
Davidroid 19:1cd7f65c155c 79
Davidroid 19:1cd7f65c155c 80 /**
Davidroid 19:1cd7f65c155c 81 * @brief This is an example of user handler for the flag interrupt.
Davidroid 19:1cd7f65c155c 82 * @param None
Davidroid 19:1cd7f65c155c 83 * @retval None
Davidroid 19:1cd7f65c155c 84 * @note If needed, implement it, and then attach and enable it:
Davidroid 19:1cd7f65c155c 85 * + motor->AttachFlagIRQ(&FlagIRQHandler);
Davidroid 19:1cd7f65c155c 86 * + motor->EnableFlagIRQ();
Davidroid 19:1cd7f65c155c 87 * To disable it:
Davidroid 19:1cd7f65c155c 88 * + motor->DisbleFlagIRQ();
Davidroid 19:1cd7f65c155c 89 */
Davidroid 19:1cd7f65c155c 90 void FlagIRQHandler(void)
Davidroid 19:1cd7f65c155c 91 {
Davidroid 19:1cd7f65c155c 92 /* Set ISR flag. */
Davidroid 19:1cd7f65c155c 93 motor->isr_flag = TRUE;
Davidroid 19:1cd7f65c155c 94
Davidroid 19:1cd7f65c155c 95 /* Get the value of the status register. */
Davidroid 19:1cd7f65c155c 96 unsigned int status = motor->GetStatus();
Davidroid 19:1cd7f65c155c 97
Davidroid 19:1cd7f65c155c 98 /* Check NOTPERF_CMD flag: if set, the command received by SPI can't be performed. */
Davidroid 19:1cd7f65c155c 99 /* This often occures when a command is sent to the L6474 while it is not in HiZ state. */
Davidroid 19:1cd7f65c155c 100 if ((status & L6474_STATUS_NOTPERF_CMD) == L6474_STATUS_NOTPERF_CMD)
Arkadi 23:ef6c5c0d33b2 101 pc.printf(" WARNING: \"FLAG\" interrupt triggered. Non-performable command detected when updating L6474's registers while not in HiZ state.\r\n");
Davidroid 19:1cd7f65c155c 102
Davidroid 19:1cd7f65c155c 103 /* Reset ISR flag. */
Davidroid 19:1cd7f65c155c 104 motor->isr_flag = FALSE;
Davidroid 19:1cd7f65c155c 105 }
Davidroid 19:1cd7f65c155c 106
Arkadi 24:77e407b5d247 107 void ServoUpdate(){
Arkadi 24:77e407b5d247 108 int32_t Rotations=motor->GetPosition()/STEPS2Rotation;
Arkadi 24:77e407b5d247 109 int32_t amplitude=76; // number of rotation for full triangle wave 15/0.4/2*4 for 15 mm translation 0.4 wire diameter =
Arkadi 24:77e407b5d247 110 float Y_New=(float) (abs(Rotations%amplitude-amplitude/2)-amplitude/4);
Arkadi 24:77e407b5d247 111 float centr_Angle=0.30; // 0.55 minimum down , 0.05 minimum up.
Arkadi 24:77e407b5d247 112 float Angle_Amplitude=0.25f;
Arkadi 24:77e407b5d247 113 float Angle=centr_Angle+(Y_New/(amplitude/4))*Angle_Amplitude;
Arkadi 24:77e407b5d247 114
Arkadi 24:77e407b5d247 115 //pc.printf("ServoUpdate %d, %d, %f \r\n",motor->GetPosition(),Rotations,Angle); // debug check/
Arkadi 24:77e407b5d247 116
Arkadi 24:77e407b5d247 117 Servo1=(Angle);
Arkadi 24:77e407b5d247 118 Servo2=(1-Angle);
Arkadi 24:77e407b5d247 119 }
Arkadi 24:77e407b5d247 120
Davidroid 19:1cd7f65c155c 121
Davidroid 0:e6a49a092e2a 122 /* Main ----------------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 123
Davidroid 0:e6a49a092e2a 124 int main()
Davidroid 0:e6a49a092e2a 125 {
Arkadi 23:ef6c5c0d33b2 126 pc.baud(57600);
Davidroid 6:32166bfc04b0 127 /*----- Initialization. -----*/
Davidroid 6:32166bfc04b0 128
Davidroid 0:e6a49a092e2a 129 /* Initializing SPI bus. */
Davidroid 0:e6a49a092e2a 130 DevSPI dev_spi(D11, D12, D13);
Davidroid 0:e6a49a092e2a 131
Davidroid 11:3e303a25770d 132 /* Initializing Motor Control Component. */
Davidroid 5:8065b587ade0 133 motor = new L6474(D2, D8, D7, D9, D10, dev_spi);
Davidroid 19:1cd7f65c155c 134 if (motor->Init(&init) != COMPONENT_OK)
Davidroid 17:4830b25fec7f 135 exit(EXIT_FAILURE);
Davidroid 0:e6a49a092e2a 136
Davidroid 19:1cd7f65c155c 137 /* Attaching and enabling interrupt handlers. */
Davidroid 19:1cd7f65c155c 138 motor->AttachFlagIRQ(&FlagIRQHandler);
Davidroid 19:1cd7f65c155c 139 motor->EnableFlagIRQ();
Davidroid 19:1cd7f65c155c 140
Davidroid 19:1cd7f65c155c 141 /*----- Changing motor setting. -----*/
Davidroid 19:1cd7f65c155c 142
Davidroid 19:1cd7f65c155c 143 /* Setting High Impedance State to update L6474's registers. */
Davidroid 19:1cd7f65c155c 144 motor->SoftHiZ();
Davidroid 19:1cd7f65c155c 145
Davidroid 19:1cd7f65c155c 146 /* Changing step mode. */
Arkadi 23:ef6c5c0d33b2 147 motor->SetStepMode(STEP_MODE_1_16); //STEP_MODE_1_16 STEP_MODE_FULL
Arkadi 23:ef6c5c0d33b2 148
Arkadi 23:ef6c5c0d33b2 149 /* Increasing the torque regulation current to 400mA. */
Arkadi 24:77e407b5d247 150 motor->SetParameter(L6474_TVAL, 1500);
Davidroid 19:1cd7f65c155c 151
Arkadi 23:ef6c5c0d33b2 152 /* Max speed to 2400 step/s. */
Arkadi 23:ef6c5c0d33b2 153 motor->SetMaxSpeed(6400);
Davidroid 6:32166bfc04b0 154
Arkadi 23:ef6c5c0d33b2 155 /* Min speed to 200 step/s. */
Arkadi 23:ef6c5c0d33b2 156 motor->SetMinSpeed(500);
Davidroid 6:32166bfc04b0 157
Arkadi 23:ef6c5c0d33b2 158 /* set accelerations */
Arkadi 23:ef6c5c0d33b2 159 motor->SetAcceleration(500);
Arkadi 23:ef6c5c0d33b2 160 motor->SetDeceleration(500);
Davidroid 6:32166bfc04b0 161
Davidroid 6:32166bfc04b0 162 /* Setting the current position to be the home position. */
Davidroid 6:32166bfc04b0 163 motor->SetHome();
Davidroid 6:32166bfc04b0 164
Arkadi 24:77e407b5d247 165 ServoCorrect.attach(&ServoUpdate,0.1f);
Arkadi 24:77e407b5d247 166
Arkadi 24:77e407b5d247 167 /* Requesting to go to a specified position. */
Arkadi 24:77e407b5d247 168 StepperSetPos=600*STEPS2Rotation;
Arkadi 24:77e407b5d247 169 motor->GoTo(StepperSetPos);
Arkadi 24:77e407b5d247 170 motor->WaitWhileActive();
Arkadi 24:77e407b5d247 171 StepperPos = motor->GetPosition();
Arkadi 24:77e407b5d247 172
Davidroid 6:32166bfc04b0 173 /* Infinite Loop. */
Arkadi 24:77e407b5d247 174 while (0)
Davidroid 6:32166bfc04b0 175 {
Arkadi 23:ef6c5c0d33b2 176
Arkadi 23:ef6c5c0d33b2 177 // receive Motor Command
Arkadi 23:ef6c5c0d33b2 178 while (pc.readable()) {
Arkadi 23:ef6c5c0d33b2 179 inbyte=pc.getc();
Arkadi 23:ef6c5c0d33b2 180 pc.printf("%c" ,inbyte); // debug check/
Arkadi 23:ef6c5c0d33b2 181 BufferCMD[BufferIndex]=inbyte;
Arkadi 23:ef6c5c0d33b2 182 BufferIndex++;
Arkadi 23:ef6c5c0d33b2 183 // parse incoming message format: "$<value>\r\n"
Arkadi 23:ef6c5c0d33b2 184 if (inbyte=='$'){ // start of message
Arkadi 23:ef6c5c0d33b2 185 BufferIndex=0; // initialize to start of parser
Arkadi 23:ef6c5c0d33b2 186 }else if(inbyte=='\r'){ // end of message
Arkadi 23:ef6c5c0d33b2 187 CMDValue=atoi(BufferCMD);
Arkadi 23:ef6c5c0d33b2 188 BufferIndex=0;
Arkadi 23:ef6c5c0d33b2 189 pc.printf("CMD: %d \r\n" ,CMDValue); // debug check/
Arkadi 23:ef6c5c0d33b2 190 StepperSetPos=CMDValue*STEPS2Rotation;
Arkadi 23:ef6c5c0d33b2 191 /* Update Stepper Commad */
Arkadi 23:ef6c5c0d33b2 192 }//end parser
Arkadi 23:ef6c5c0d33b2 193 }//end serial
Arkadi 23:ef6c5c0d33b2 194
Davidroid 6:32166bfc04b0 195 /* Requesting to go to a specified position. */
Arkadi 23:ef6c5c0d33b2 196 motor->GoTo(StepperSetPos);
Davidroid 1:fbf28f3367aa 197 motor->WaitWhileActive();
Arkadi 23:ef6c5c0d33b2 198 StepperPos = motor->GetPosition();
Davidroid 0:e6a49a092e2a 199
Arkadi 23:ef6c5c0d33b2 200 pc.printf("Rotations: %d \r\n", StepperPos/STEPS2Rotation);
Arkadi 23:ef6c5c0d33b2 201 wait(1);
Davidroid 0:e6a49a092e2a 202 }
Davidroid 0:e6a49a092e2a 203 }