Jeroen Lodder / Mbed 2 deprecated StepperMotor_Bipolar_Microstepping

Full H Bridge Microstepping Stepper Motor Demo software

Dependencies:   mbed

main.cpp@0:8e4a6920a484, 2013-10-08 (annotated)

Committer:
jeroen3
Date:
Tue Oct 08 19:51:11 2013 +0000
Revision:
0:8e4a6920a484
Initial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jeroen3 0:8e4a6920a484 1 /**
jeroen3 0:8e4a6920a484 2 * @file main.cpp
jeroen3 0:8e4a6920a484 3 * @brief Bipolar Steppr Motor Control with dual H Bridge
jeroen3 0:8e4a6920a484 4 * @details Software plays stepper motor lookup tables for dual h bridge control.
jeroen3 0:8e4a6920a484 5 *
jeroen3 0:8e4a6920a484 6 *
jeroen3 0:8e4a6920a484 7 * @Author Jeroen Lodder
jeroen3 0:8e4a6920a484 8 * @Date September 2013
jeroen3 0:8e4a6920a484 9 * @version 1
jeroen3 0:8e4a6920a484 10 *
jeroen3 0:8e4a6920a484 11 */
jeroen3 0:8e4a6920a484 12
jeroen3 0:8e4a6920a484 13 #include "mbed.h"
jeroen3 0:8e4a6920a484 14 #include "smclut.h"
jeroen3 0:8e4a6920a484 15
jeroen3 0:8e4a6920a484 16
jeroen3 0:8e4a6920a484 17 /* MOSFET io
jeroen3 0:8e4a6920a484 18 * FET L1 L2 H1 H2 H3 H4 L3 L4
jeroen3 0:8e4a6920a484 19 * Motor MA MB
jeroen3 0:8e4a6920a484 20 * mbed p21 p22 p25 p26 p27 p28 p23 p24
jeroen3 0:8e4a6920a484 21 * ------------------------ V+
jeroen3 0:8e4a6920a484 22 * | | | |
jeroen3 0:8e4a6920a484 23 * \H1 \H2 \H3 \H4
jeroen3 0:8e4a6920a484 24 * |--MA--| |--MB--|
jeroen3 0:8e4a6920a484 25 * \L1 \L2 \L3 \L4
jeroen3 0:8e4a6920a484 26 * | | | |
jeroen3 0:8e4a6920a484 27 * ------------------------ V-
jeroen3 0:8e4a6920a484 28 */
jeroen3 0:8e4a6920a484 29
jeroen3 0:8e4a6920a484 30 /* Fet driver definitions */
jeroen3 0:8e4a6920a484 31 /* Turn off time fet driver in micro seconds (uS) */
jeroen3 0:8e4a6920a484 32 #define TURN_ON_TIME_FET_DRIVER 750
jeroen3 0:8e4a6920a484 33
jeroen3 0:8e4a6920a484 34 /* Control definitions */
jeroen3 0:8e4a6920a484 35 /* Mutliplier of speed wait time setting */
jeroen3 0:8e4a6920a484 36 #define SPEED_SCALING 0.5
jeroen3 0:8e4a6920a484 37
jeroen3 0:8e4a6920a484 38 /* Motor control definitions */
jeroen3 0:8e4a6920a484 39 #define PWM_PERIOD 512 // Q15
jeroen3 0:8e4a6920a484 40 #define PWM_AMPLITIUDE 1 // 0 to 1, equals torque
jeroen3 0:8e4a6920a484 41 #define PHASE_SHIFT 1 // Shift in rads
jeroen3 0:8e4a6920a484 42
jeroen3 0:8e4a6920a484 43 #define PI 3.14159265359
jeroen3 0:8e4a6920a484 44
jeroen3 0:8e4a6920a484 45 AnalogOut signal(p18);
jeroen3 0:8e4a6920a484 46 Serial pc(USBTX, USBRX);
jeroen3 0:8e4a6920a484 47
jeroen3 0:8e4a6920a484 48 /* IO declarations */
jeroen3 0:8e4a6920a484 49 /* LED coil indicators */
jeroen3 0:8e4a6920a484 50 DigitalOut LED_H1(LED1);
jeroen3 0:8e4a6920a484 51 DigitalOut LED_H2(LED2);
jeroen3 0:8e4a6920a484 52 DigitalOut LED_H3(LED3);
jeroen3 0:8e4a6920a484 53 DigitalOut LED_H4(LED4);
jeroen3 0:8e4a6920a484 54 #define SetLed() LED_H1 = FET_H1; \
jeroen3 0:8e4a6920a484 55 LED_H2 = FET_H2; \
jeroen3 0:8e4a6920a484 56 LED_H3 = FET_H3; \
jeroen3 0:8e4a6920a484 57 LED_H4 = FET_H4;
jeroen3 0:8e4a6920a484 58
jeroen3 0:8e4a6920a484 59 /* H Bridge output IO */
jeroen3 0:8e4a6920a484 60 PwmOut FET_L1( p21 ); // MA
jeroen3 0:8e4a6920a484 61 PwmOut FET_L2( p22 ); // MA
jeroen3 0:8e4a6920a484 62
jeroen3 0:8e4a6920a484 63 PwmOut FET_L3( p23 ); // MB
jeroen3 0:8e4a6920a484 64 PwmOut FET_L4( p24 ); // MB
jeroen3 0:8e4a6920a484 65
jeroen3 0:8e4a6920a484 66 DigitalOut FET_H1( p25 ); // MA
jeroen3 0:8e4a6920a484 67 DigitalOut FET_H2( p26 ); // MA
jeroen3 0:8e4a6920a484 68
jeroen3 0:8e4a6920a484 69 DigitalOut FET_H3( p27 ); // MB
jeroen3 0:8e4a6920a484 70 DigitalOut FET_H4( p28 ); // MB
jeroen3 0:8e4a6920a484 71
jeroen3 0:8e4a6920a484 72 /* Motor Control Ticker */
jeroen3 0:8e4a6920a484 73 Ticker smc;
jeroen3 0:8e4a6920a484 74 volatile int smc_walker = 0;
jeroen3 0:8e4a6920a484 75 volatile int smc_dir = 1;
jeroen3 0:8e4a6920a484 76 volatile int smc_steps = -1;
jeroen3 0:8e4a6920a484 77 volatile int smc_free = 1;
jeroen3 0:8e4a6920a484 78
jeroen3 0:8e4a6920a484 79
jeroen3 0:8e4a6920a484 80 void smc_routine(){
jeroen3 0:8e4a6920a484 81 #define i smc_walker
jeroen3 0:8e4a6920a484 82
jeroen3 0:8e4a6920a484 83 // Phase 1 A
jeroen3 0:8e4a6920a484 84 // If sin +, H1->L2
jeroen3 0:8e4a6920a484 85 // If sin -, H2->L1
jeroen3 0:8e4a6920a484 86 FET_H1 = LUT_H1[i];
jeroen3 0:8e4a6920a484 87 FET_H2 = LUT_H2[i];
jeroen3 0:8e4a6920a484 88 FET_L1.pulsewidth_us( LUT_L1[i] );
jeroen3 0:8e4a6920a484 89 FET_L2.pulsewidth_us( LUT_L2[i] );
jeroen3 0:8e4a6920a484 90
jeroen3 0:8e4a6920a484 91 // Phase 1 A
jeroen3 0:8e4a6920a484 92 // If sin +, H1->L2
jeroen3 0:8e4a6920a484 93 // If sin -, H2->L1
jeroen3 0:8e4a6920a484 94 FET_H3 = LUT_H3[i];
jeroen3 0:8e4a6920a484 95 FET_H4 = LUT_H4[i];
jeroen3 0:8e4a6920a484 96 FET_L3.pulsewidth_us( LUT_L3[i] );
jeroen3 0:8e4a6920a484 97 FET_L4.pulsewidth_us( LUT_L4[i] );
jeroen3 0:8e4a6920a484 98
jeroen3 0:8e4a6920a484 99 // uint8_t bridge = ( FET_H1.read() << 7 )|
jeroen3 0:8e4a6920a484 100 // ( isPositive(FET_L1.read()) << 6 )|
jeroen3 0:8e4a6920a484 101 // ( FET_H2.read() << 5 )|
jeroen3 0:8e4a6920a484 102 // ( isPositive(FET_L2.read()) << 4 )|
jeroen3 0:8e4a6920a484 103 // ( FET_H3.read() << 3 )|
jeroen3 0:8e4a6920a484 104 // ( isPositive(FET_L3.read()) << 2 )|
jeroen3 0:8e4a6920a484 105 // ( FET_H4.read() << 1 )|
jeroen3 0:8e4a6920a484 106 // ( isPositive(FET_L4.read()) << 0 );
jeroen3 0:8e4a6920a484 107 SetLed();
jeroen3 0:8e4a6920a484 108 #undef i
jeroen3 0:8e4a6920a484 109
jeroen3 0:8e4a6920a484 110 /* Walk */
jeroen3 0:8e4a6920a484 111 smc_walker += smc_dir;
jeroen3 0:8e4a6920a484 112 if(smc_walker > 31)
jeroen3 0:8e4a6920a484 113 smc_walker = 0;
jeroen3 0:8e4a6920a484 114 if(smc_walker < 0)
jeroen3 0:8e4a6920a484 115 smc_walker = 31;
jeroen3 0:8e4a6920a484 116 /* Coutdown */
jeroen3 0:8e4a6920a484 117 if(smc_steps != -1){
jeroen3 0:8e4a6920a484 118 if(smc_steps == 0){
jeroen3 0:8e4a6920a484 119 if(smc_free){
jeroen3 0:8e4a6920a484 120 // motor free
jeroen3 0:8e4a6920a484 121 FET_L1 = 1;
jeroen3 0:8e4a6920a484 122 FET_L2 = 1;
jeroen3 0:8e4a6920a484 123 FET_L3 = 1;
jeroen3 0:8e4a6920a484 124 FET_L4 = 1;
jeroen3 0:8e4a6920a484 125 FET_H1 = 0;
jeroen3 0:8e4a6920a484 126 FET_H2 = 0;
jeroen3 0:8e4a6920a484 127 FET_H3 = 0;
jeroen3 0:8e4a6920a484 128 FET_H4 = 0;
jeroen3 0:8e4a6920a484 129 }else{
jeroen3 0:8e4a6920a484 130 // motor locked
jeroen3 0:8e4a6920a484 131 }
jeroen3 0:8e4a6920a484 132 SetLed();
jeroen3 0:8e4a6920a484 133 smc.detach();
jeroen3 0:8e4a6920a484 134 }
jeroen3 0:8e4a6920a484 135 smc_steps--;
jeroen3 0:8e4a6920a484 136 }
jeroen3 0:8e4a6920a484 137 }
jeroen3 0:8e4a6920a484 138
jeroen3 0:8e4a6920a484 139 void smc_dostep(int steps, int dir, float time_ms, int free){
jeroen3 0:8e4a6920a484 140 // steps = number of microsteps (8 microsteps per full step)
jeroen3 0:8e4a6920a484 141 // dir = -1 or 1
jeroen3 0:8e4a6920a484 142 // time_us = completion time in s
jeroen3 0:8e4a6920a484 143 smc_steps = steps;
jeroen3 0:8e4a6920a484 144 smc_dir = dir;
jeroen3 0:8e4a6920a484 145 smc_free = free;
jeroen3 0:8e4a6920a484 146 smc.attach_us(&smc_routine, (time_ms*1000)/steps);
jeroen3 0:8e4a6920a484 147 }
jeroen3 0:8e4a6920a484 148
jeroen3 0:8e4a6920a484 149
jeroen3 0:8e4a6920a484 150 int main(void) {
jeroen3 0:8e4a6920a484 151 pc.baud(115200);
jeroen3 0:8e4a6920a484 152 pc.printf("hi\r\n");
jeroen3 0:8e4a6920a484 153
jeroen3 0:8e4a6920a484 154 /* Control declarations */
jeroen3 0:8e4a6920a484 155 /* Direction LUT are played */
jeroen3 0:8e4a6920a484 156 //DigitalIn direction( p20 );
jeroen3 0:8e4a6920a484 157 /* Step pulse input if step mode */
jeroen3 0:8e4a6920a484 158 //DigitalIn step( p18 );
jeroen3 0:8e4a6920a484 159 /* Speed potmeter input */
jeroen3 0:8e4a6920a484 160 //AnalogIn speed( p19 );
jeroen3 0:8e4a6920a484 161
jeroen3 0:8e4a6920a484 162 //AnalogIn phase( p17 );
jeroen3 0:8e4a6920a484 163
jeroen3 0:8e4a6920a484 164 /* PWM init */
jeroen3 0:8e4a6920a484 165 FET_L1.period_us(PWM_PERIOD);
jeroen3 0:8e4a6920a484 166 FET_L2.period_us(PWM_PERIOD);
jeroen3 0:8e4a6920a484 167 FET_L3.period_us(PWM_PERIOD);
jeroen3 0:8e4a6920a484 168 FET_L4.period_us(PWM_PERIOD);
jeroen3 0:8e4a6920a484 169 FET_L1 = 1;
jeroen3 0:8e4a6920a484 170 FET_L2 = 1;
jeroen3 0:8e4a6920a484 171 FET_L3 = 1;
jeroen3 0:8e4a6920a484 172 FET_L4 = 1;
jeroen3 0:8e4a6920a484 173 FET_H1 = 0;
jeroen3 0:8e4a6920a484 174 FET_H2 = 0;
jeroen3 0:8e4a6920a484 175 FET_H3 = 0;
jeroen3 0:8e4a6920a484 176 FET_H4 = 0;
jeroen3 0:8e4a6920a484 177 SetLed();
jeroen3 0:8e4a6920a484 178
jeroen3 0:8e4a6920a484 179 //smc.attach_us(&smc_routine, 6000);
jeroen3 0:8e4a6920a484 180 smc_dostep(1600, 1, 1000.0, 1);
jeroen3 0:8e4a6920a484 181
jeroen3 0:8e4a6920a484 182
jeroen3 0:8e4a6920a484 183 /* Stepper motor control loop */
jeroen3 0:8e4a6920a484 184 while(1){
jeroen3 0:8e4a6920a484 185 while(!pc.readable());
jeroen3 0:8e4a6920a484 186 while(pc.readable()) pc.getc();
jeroen3 0:8e4a6920a484 187 smc_dostep(16, 1, 100.0, 0);
jeroen3 0:8e4a6920a484 188 }
jeroen3 0:8e4a6920a484 189 }