DCAITI Robot Hardware  1.0
MotorWheel.h
Go to the documentation of this file.
1 
2 /*
3 V1.1 201104
4 1. motorwheel library version 1.1,compatible with maple.
5 
6 V1.1.1 201110
7 1. Add Acceleration
8 
9 V1.2 201207
10 1. Double CPR from 12 to 24, Interrupt Type from RISING to CHANGE.
11 2. Reduce default sample time from 10ms to 5ms.
12 3. Compatible with Namiki 22CL-3501PG80:1, by "#define _NAMIKI_MOTOR" before "#include ...".
13 
14 V1.3 201209
15 1. R2WD::delayMS(), Omni3WD::delayMS(), Omni4WD::delayMS() are re-implemented, more exactly.
16 2. getSpeedRPM(), setSpeedRPM(), getSpeedMMPS(), setSpeedMMPS() are plug-minus/direction sensitive now.
17 3. Acceleration is disabled.
18 
19 V1.4 201209 Not Released
20 1. Increase CPR from 24 to 48 for Faulhaber 2342.
21 
22 V1.5 201209 Omni4WD is re-implemented, and now return value of Omni4WD::getSpeedMMPS() is correct.
23 
24  */
25 
26 /*for maple*/
27 #if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini)
28  #include "wirish.h"
29  #include "./../PID_Beta6/PID_Beta6.h"
30  #include "ext_interrupts.h"
31 
32 /*for arduino*/
33 #else
34  //#include<Arduino.h>
35  //#include<WProgram.h>
36  #include<PID_Beta6.h>
37  #include<PinChangeInt.h>
38 
39 #endif
40 
41 /*
42 
43  class PID
44  class Motor
45  class GearedMotor
46  class MotorWheel
47 
48  */
49 
50 #ifndef MotorWheel_H
51 #define MotorWheel_H
52 
53 #define DIR_ADVANCE HIGH
54 #define DIR_BACKOFF LOW
55 
56 #define PIN_UNDEFINED 255
57 #define REF_VOLT 12
58 /*for maple*/
59 #if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini)
60  #define MAX_PWM 3599
61 
62 /*for arduino*/
63 #else
64  #define MAX_PWM 255
65 
66 #endif
67 
68 #ifdef _NAMIKI_MOTOR
69  #define TRIGGER CHANGE
70  #define CPR 4 // Namiki motor
71  #define DIR_INVERSE
72  #define REDUCTION_RATIO 80
73 #else
74  #define TRIGGER CHANGE
75  #define CPR 24 // Faulhaber motor
76  #define DIR_INVERSE !
77  #define REDUCTION_RATIO 64
78 #endif
79 
80 #define MAX_SPEEDRPM 8000
81 //#define DESIRED_SPEEDRPM 3000
82 //#define SPEED_CONSTANT_RPM 713
83 //#define PWM2SPEED_RESOLUTION 143 //713 * 0.2
84 
85 #define SEC_PER_MIN 60
86 #define MICROS_PER_SEC 1000000
87 //#define SPEEDPPS2SPEEDRPM(freq) ((freq)*((SEC_PER_MIN)/(CPR))) //(freq*SEC_PER_MIN/CPR)
88 #define SPEEDPPS2SPEEDRPM(freq) ((unsigned long)(freq)*(SEC_PER_MIN)/(CPR)) //(freq*SEC_PER_MIN/CPR)
89 //#define SPEEDPPS2SPEED2VOLT(spd) ((spd)/SPEED_CONSTANT_RPM)
90 //#define MAX_SPEEDPPS ((MAX_SPEEDRPM*CPR)/SEC_PER_MIN)
91 //#define MIN_SPEEDPPS 0
92 
93 #define KC 0.31
94 #define TAUI 0.02
95 #define TAUD 0.00
96 #define SAMPLETIME 5
97 
98 #define Baudrate 19200
99 /*
100 #ifndef DEBUG
101 #define DEBUG
102 #endif
103  */
104 #ifdef DEBUG
105 #define debug() { \
106  if(!Serial.available()) Serial.begin(Baudrate); \
107  Serial.println(__func__);}
108 #else
109 #define debug() {}
110 #endif
111 
112 
113 // Interrupt Type: RISING --> CHANGE 201207
114 #define irqISR(y,x) \
115  void x(); \
116  struct ISRVars y={x}; \
117  void x() { \
118  static bool first_pulse=true; \
119  y.pulseEndMicros=micros(); \
120  if(first_pulse==false && y.pulseEndMicros>y.pulseStartMicros) { \
121  y.speedPPS=MICROS_PER_SEC/(y.pulseEndMicros-y.pulseStartMicros); \
122  /* y.accPPSS=(y.speedPPS-y.lastSpeedPPS)*y.speedPPS; */ \
123  } else first_pulse=false; \
124  y.pulseStartMicros=y.pulseEndMicros; \
125  /* y.lastSpeedPPS=y.speedPPS; */ \
126  if(y.pinIRQB!=PIN_UNDEFINED) \
127  y.currDirection=DIR_INVERSE(digitalRead(y.pinIRQ)^digitalRead(y.pinIRQB)); \
128  y.currDirection==DIR_ADVANCE?++y.pulses:--y.pulses; \
129  }
130 
131 struct ISRVars {
132  void (*ISRfunc)();
133  //volatile unsigned long pulses;
134  volatile long pulses; // 201104, direction sensitive
135  volatile unsigned long pulseStartMicros;
136  volatile unsigned long pulseEndMicros;
137  volatile unsigned int speedPPS;
138  //volatile unsigned int lastSpeedPPS;
139  //volatile int accPPSS; // acceleration, Pulse Per Sec^2
140  volatile bool currDirection;
141  unsigned char pinIRQB;
142  unsigned char pinIRQ; // pinIRQA 201207
143 };
144 
145 class Motor: public PID {
146 public:
147  Motor(unsigned char _pinPWM,unsigned char _pinDir,
148  unsigned char _pinIRQ,unsigned char _pinIRQB,
149  struct ISRVars* _isr);
150 
151  void setupInterrupt();
152 
153  unsigned char getPinPWM() const;
154  unsigned char getPinDir() const;
155  unsigned char getPinIRQ() const;
156  unsigned char getPinIRQB() const;
157 
158  unsigned int runPWM(unsigned int PWM,bool dir,bool saveDir=true);
159  unsigned int getPWM() const;
160  unsigned int advancePWM(unsigned int PWM);
161  unsigned int backoffPWM(unsigned int PWM);
162 
163  bool setDesiredDir(bool dir);
164  bool getDesiredDir() const;
165  bool reverseDesiredDir();
166 
167  bool setCurrDir();
168  bool getCurrDir() const;
169 
170  //int getAccRPMM() const; // Acceleration, Round Per Min^2
171  //unsigned int getSpeedRPM() const;
172  //unsigned int setSpeedRPM(int speedRPM,bool dir);
173  // direction sensitive 201208
174  int getSpeedRPM() const;
175  unsigned int setSpeedRPM(int speedRPM,bool dir); // preserve
176  int setSpeedRPM(int speedRPM);
177 
178  //void simpleRegulate();
179 
180  bool PIDSetup(float kc=KC,float taui=TAUI,float taud=TAUD,unsigned int sampleTime=1000);
181  bool PIDGetStatus() const;
182  bool PIDEnable(float kc=KC,float taui=TAUI,float taud=TAUD,unsigned int sampleTime=1000);
183  bool PIDDisable();
184  bool PIDReset();
185  bool PIDRegulate(bool doRegulate=true);
186  unsigned int PIDSetSpeedRPMDesired(unsigned int speedRPM);
187  unsigned int PIDGetSpeedRPMDesired() const;
188 
189  void delayMS(unsigned int ms,bool debug=false);
190  void debugger() const;
191 
192  //int getAccPPSS() const;
193  int getSpeedPPS() const;
194  long getCurrPulse() const;
195  long setCurrPulse(long _pulse);
196  long resetCurrPulse();
197 
198  struct ISRVars* isr;
199 
200 private:
201  unsigned char pinPWM;
202  unsigned char pinDir;
203  //unsigned char pinIRQ; // moved to isr
204  //unsigned char pinIRQB;
205 
206  //bool currDirection; // current direction
207  bool desiredDirection; // desired direction
208  unsigned int speedPWM; // current PWM
209 
210  int speedRPMInput; // RPM: Round Per Minute
211  int speedRPMOutput; // RPM
212  int speedRPMDesired; // RPM
213  //float PWMEC;
214  float speed2DutyCycle;
215 /*
216  // the followings are defined in struct ISRvars,
217  // because ISR must be a global function, without parameters and no return value
218 
219  volatile unsigned int speedPPS; // PPS: Pulses Per Second
220  volatile unsigned long pulseStartMicros;
221  volatile unsigned long pulseEndMicros;
222  */
223  bool pidCtrl;
224 
225  Motor();
226 
227 };
228 
229 
230 #ifndef REDUCTION_RATIO
231 #define REDUCTION_RATIO 64
232 #endif
233 class GearedMotor: public Motor { // RPM
234 public:
235  GearedMotor(unsigned char _pinPWM,unsigned char _pinDir,
236  unsigned char _pinIRQ,unsigned char _pinIRQB,
237  struct ISRVars* _isr,
238  unsigned int _ratio=REDUCTION_RATIO);
239  //float getGearedAccRPMM() const; // Acceleration, Round Per Min^2
240  float getGearedSpeedRPM() const;
241  float setGearedSpeedRPM(float gearedSpeedRPM,bool dir);
242  // direction sensitive 201208
243  float setGearedSpeedRPM(float gearedSpeedRPM);
244  unsigned int getRatio() const;
245  unsigned int setRatio(unsigned int ratio=REDUCTION_RATIO);
246  float getPosition();
247 private:
248  unsigned int _ratio;
249 };
250 
251 
252 #ifndef PI
253 #define PI 3.1416
254 #endif
255 //#define CIR 31.4 // cm
256 //#define CIRMM 150 // mm
257 //#define CIRMM 188
258 #define CIRMM 314
259 class MotorWheel: public GearedMotor { //
260 public:
261  MotorWheel(unsigned char _pinPWM,unsigned char _pinDir,
262  unsigned char _pinIRQ,unsigned char _pinIRQB,
263  struct ISRVars* _isr,
264  unsigned int ratio=REDUCTION_RATIO,unsigned int cirMM=CIRMM);
265 
266  unsigned int getCirMM() const;
267  unsigned int setCirMM(unsigned int cirMM=CIRMM);
268 /*
269  int getAccCMPMM() const; // Acceleration, CM Per Min^2
270  unsigned int getSpeedCMPM() const; // cm/min
271  unsigned int setSpeedCMPM(unsigned int cm,bool dir);
272  int getAccMMPSS() const; // Acceleration, MM Per Sec^2
273  unsigned int getSpeedMMPS() const; // mm/s
274  unsigned int setSpeedMMPS(unsigned int mm,bool dir);
275  */
276  // direction sensitive 201208
277  //int getAccCMPMM() const; // Acceleration, CM Per Min^2
278  int getSpeedCMPM() const; // cm/min
279  int setSpeedCMPM(unsigned int cm,bool dir); // preserve
280  int setSpeedCMPM(int cm);
281  //int getAccMMPSS() const; // Acceleration, MM Per Sec^2
282  int getSpeedMMPS() const; // mm/s
283  int setSpeedMMPS(unsigned int mm,bool dir); // preserve
284  int setSpeedMMPS(int mm);
285  void setGearedRPM(float rpm, bool dir);
286 
287  //unsigned int runTime(unsigned int speedMMPS,bool dir,unsigned int TimeMS);
288  //unsigned int runDistance(unsigned int speedMMPS,bool dir,unsigned int distanceMM);
289 
290 private:
291  unsigned int _cirMM;
292 };
293 
294 /*
295 class samePID: public PID {
296 public:
297  samePID(int* input,int* output,int* setPoint,float kc,float taui,float taud)
298  :PID(input,output,setPoint,kc,taui,taud) { }
299 };
300 
301 class servoMotor: public Motor,public samePID {
302 public:
303  servoMotor(unsigned char _pinPWM,unsigned char _pinDir,
304  unsigned char _pinIRQ,unsigned char _pinIRQB,
305  struct ISRVars* _isr);
306 
307  int SPIDSetPulseBoundary(int min,int max);
308  bool SPIDSetup(float kc=KC,float taui=TAUI,float taud=TAUD,unsigned int sampleTime=10);
309  bool PIDRegulate(bool doRegulate=true);
310 
311  enum {
312  SPIDMODE_UNKNOWN,
313  SPIDMODE_SPEED,
314  SPIDMODE_STOP,
315  SPIDMODE_PULSE,
316  };
317  unsigned char getSPIDMode() const;
318  unsigned char setSPIDMode(unsigned char _mode);
319 
320  int getPulseDesired() const;
321  int setPulseDesired(int _pulse);
322  int incPulseDesired();
323  int decPulseDesired();
324 
325  bool backToZero();
326  bool scanZero();
327 
328  void debugger() const;
329 
330 private:
331  int pulseInput; //
332  int pulseOutput; //
333  int pulseDesired; //
334 
335  int pulse2SpeedRPM;
336 
337  unsigned char SPIDMode;
338 
339  bool SPIDRegulateSpeed(bool doRegulate=true); // 201104
340  bool SPIDRegulateStop(bool doRegulate=true);
341  bool SPIDRegulatePulse(bool doRegulate=true);
342 
343 };
344  */
345 
346 
347 
348 #endif
349 
#define REDUCTION_RATIO
Definition: MotorWheel.h:77
#define CIRMM
Definition: MotorWheel.h:258
#define KC
Definition: MotorWheel.h:93
#define TAUI
Definition: MotorWheel.h:94
#define TAUD
Definition: MotorWheel.h:95
#define debug()
Definition: MotorWheel.h:109
Definition: MotorWheel.h:233
float setGearedSpeedRPM(float gearedSpeedRPM, bool dir)
Definition: MotorWheel.cpp:309
float getGearedSpeedRPM() const
Definition: MotorWheel.cpp:303
unsigned int setRatio(unsigned int ratio=REDUCTION_RATIO)
Definition: MotorWheel.cpp:293
float getPosition()
Definition: MotorWheel.cpp:323
GearedMotor(unsigned char _pinPWM, unsigned char _pinDir, unsigned char _pinIRQ, unsigned char _pinIRQB, struct ISRVars *_isr, unsigned int _ratio=REDUCTION_RATIO)
Definition: MotorWheel.cpp:284
unsigned int getRatio() const
Definition: MotorWheel.cpp:290
Definition: MotorWheel.h:259
void setGearedRPM(float rpm, bool dir)
Definition: MotorWheel.cpp:364
int setSpeedMMPS(unsigned int mm, bool dir)
Definition: MotorWheel.cpp:375
MotorWheel(unsigned char _pinPWM, unsigned char _pinDir, unsigned char _pinIRQ, unsigned char _pinIRQB, struct ISRVars *_isr, unsigned int ratio=REDUCTION_RATIO, unsigned int cirMM=CIRMM)
Definition: MotorWheel.cpp:329
int getSpeedMMPS() const
Definition: MotorWheel.cpp:370
unsigned int setCirMM(unsigned int cirMM=CIRMM)
Definition: MotorWheel.cpp:340
int getSpeedCMPM() const
Definition: MotorWheel.cpp:345
int setSpeedCMPM(unsigned int cm, bool dir)
Definition: MotorWheel.cpp:350
unsigned int getCirMM() const
Definition: MotorWheel.cpp:337
Definition: MotorWheel.h:145
bool PIDSetup(float kc=KC, float taui=TAUI, float taud=TAUD, unsigned int sampleTime=1000)
Definition: MotorWheel.cpp:171
void delayMS(unsigned int ms, bool debug=false)
Definition: MotorWheel.cpp:230
unsigned int advancePWM(unsigned int PWM)
Definition: MotorWheel.cpp:80
long resetCurrPulse()
Definition: MotorWheel.cpp:226
bool PIDGetStatus() const
Definition: MotorWheel.cpp:149
bool setDesiredDir(bool dir)
Definition: MotorWheel.cpp:92
unsigned int PIDSetSpeedRPMDesired(unsigned int speedRPM)
Definition: MotorWheel.cpp:180
unsigned char getPinIRQB() const
Definition: MotorWheel.cpp:67
long getCurrPulse() const
Definition: MotorWheel.cpp:219
void debugger() const
Definition: MotorWheel.cpp:240
int getSpeedPPS() const
Definition: MotorWheel.cpp:216
bool PIDDisable()
Definition: MotorWheel.cpp:159
long setCurrPulse(long _pulse)
Definition: MotorWheel.cpp:222
unsigned char getPinIRQ() const
Definition: MotorWheel.cpp:63
void setupInterrupt()
Definition: MotorWheel.cpp:33
unsigned int setSpeedRPM(int speedRPM, bool dir)
Definition: MotorWheel.cpp:129
bool setCurrDir()
Definition: MotorWheel.cpp:111
unsigned int PIDGetSpeedRPMDesired() const
Definition: MotorWheel.cpp:186
unsigned int getPWM() const
Definition: MotorWheel.cpp:88
bool PIDRegulate(bool doRegulate=true)
Definition: MotorWheel.cpp:191
bool getDesiredDir() const
Definition: MotorWheel.cpp:98
bool getCurrDir() const
Definition: MotorWheel.cpp:108
struct ISRVars * isr
Definition: MotorWheel.h:198
unsigned char getPinPWM() const
Definition: MotorWheel.cpp:55
unsigned int runPWM(unsigned int PWM, bool dir, bool saveDir=true)
Definition: MotorWheel.cpp:72
bool PIDEnable(float kc=KC, float taui=TAUI, float taud=TAUD, unsigned int sampleTime=1000)
Definition: MotorWheel.cpp:153
int getSpeedRPM() const
Definition: MotorWheel.cpp:136
bool PIDReset()
Definition: MotorWheel.cpp:164
bool reverseDesiredDir()
Definition: MotorWheel.cpp:102
unsigned int backoffPWM(unsigned int PWM)
Definition: MotorWheel.cpp:84
unsigned char getPinDir() const
Definition: MotorWheel.cpp:59
Definition: PID_Beta6.h:5
Definition: MotorWheel.h:131
unsigned char pinIRQB
Definition: MotorWheel.h:141
void(* ISRfunc)()
Definition: MotorWheel.h:132
volatile long pulses
Definition: MotorWheel.h:134
volatile unsigned long pulseEndMicros
Definition: MotorWheel.h:136
volatile bool currDirection
Definition: MotorWheel.h:140
volatile unsigned long pulseStartMicros
Definition: MotorWheel.h:135
unsigned char pinIRQ
Definition: MotorWheel.h:142
volatile unsigned int speedPPS
Definition: MotorWheel.h:137