forked from jpearman/smartMotorLib
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathSmartMotorLib.c
executable file
·1878 lines (1576 loc) · 71.9 KB
/
SmartMotorLib.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/*-----------------------------------------------------------------------------*/
/* */
/* Copyright (c) James Pearman */
/* 2012 */
/* All Rights Reserved */
/* */
/*-----------------------------------------------------------------------------*/
/* */
/* Module: SmartMotorLib.c */
/* Author: James Pearman */
/* Created: 2 Oct 2012 */
/* */
/* Revisions: */
/* V1.00 21 Oct 2012 - Initial release */
/* V1.01 7 Dec 2012 */
/* small bug in SmartMotorLinkMotors */
/* fix for High Speed 393 Ke constant */
/* kNumbOfTotalMotors replaced with kNumbOfRealMotors */
/* _Target_Emulator_ defined for versions of ROBOTC */
/* prior to 3.55 */
/* change to motor enums for V3.60 ROBOTC compatibility */
/* V1.02 27 Jan 2013 */
/* Linking an encoded and non-encoded motor was not */
/* working correctly, added new field to the structure */
/* eport to allow one motor to access the encoder for */
/* another correctly. */
/* V1.03 10 March 2013 */
/* Due to new version of ROBOTC (V3.60) detection of PID */
/* version changed. V3.60 was originally planned to have */
/* different motor definitions. */
/* Added the ability to assign any sensor to be used */
/* for rpm calculation, a bit of a kludge as I didn't */
/* want to add a new variable so reused encoder_id */
/* V1.04 27 June 2013 */
/* Change license (added) to the Apache License */
/* V1.05 11 Nov 2013 */
/* Fix bug when speed limited and changing directions */
/* quickly. */
/* V1.06 3 Sept 2014 */
/* Added support for the 393 Turbo Gears and ROBOTC V4.26 */
/*-----------------------------------------------------------------------------*/
/* */
/* The author is supplying this software for use with the VEX cortex */
/* control system. This file can be freely distributed and teams are */
/* authorized to freely use this program , however, it is requested that */
/* improvements or additions be shared with the Vex community via the vex */
/* forum. Please acknowledge the work of the authors when appropriate. */
/* Thanks. */
/* */
/* Licensed under the Apache License, Version 2.0 (the "License"); */
/* you may not use this file except in compliance with the License. */
/* You may obtain a copy of the License at */
/* */
/* http://www.apache.org/licenses/LICENSE-2.0 */
/* */
/* Unless required by applicable law or agreed to in writing, software */
/* distributed under the License is distributed on an "AS IS" BASIS, */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. */
/* See the License for the specific language governing permissions and */
/* limitations under the License. */
/* */
/* Portions of this code are based on work by Chris Siegert aka vamfun on */
/* the Vex forums. */
/* blog: vamfun.wordpress.com for model details and vex forum threads */
/* email: vamfun_at_yahoo_dot_com */
/* Mentor for team 599 Robodox and team 1508 Lancer Bots */
/* */
/* The author can be contacted on the vex forums as jpearman */
/* email: jbpearman_at_mac_dot_com */
/* Mentor for team 8888 RoboLancers, Pasadena CA. */
/* */
/*-----------------------------------------------------------------------------*/
/* */
/* Description: */
/* */
/* This library is designed to work with encoded motors and provides */
/* functions to obtain velocity, instantaneous current and estimated PTC */
/* temperature. This data is then used to allow fixed threshold or */
/* temperature based current limiting. */
/* */
/* The algorithms used are based on experiments and work done during the */
/* summer of 2012 by Chris Siegert and James Pearman. */
/* */
/* This library makes extensive use of pointers and therefore needs ROBOTC */
/* V3.51 or later. */
/* */
/* The following vexforum threads have much of the background information */
/* used in this library. */
/* http://www.vexforum.com/showthread.php?t=72100 */
/* http://www.vexforum.com/showthread.php?t=73318 */
/* http://www.vexforum.com/showthread.php?t=73960 */
/* http://www.vexforum.com/showthread.php?t=74594 */
/* */
/* Global Memory use for V1.02 is 1404 bytes */
/* 1240 for motor data */
/* 156 for controller data */
/* 8 misc */
/* */
/* CPU time for SmartMotorTask */
/* Motor calculations ~ 530uS, approx 5% cpu bandwidth */
/* Controller calculations with LED status ~ 1.25mS */
/* Worse case is therefore about 1.8mS which occurs every 100mS */
/* */
/* CPU time for SmartMotorSlewRateTask */
/* approx 400uS per 15mS loop, about 3% cpu bandwidth */
/* */
/*-----------------------------------------------------------------------------*/
#ifndef __SMARTMOTORLIB__
#define __SMARTMOTORLIB__
// Version 1.05
#define kSmartMotorLibVersion 106
// We make extensive use of pointers so need recent versions of ROBOTC
#include "FirmwareVersion.h"
#if kRobotCVersionNumeric < 351
#error "SmartMotorLib requires ROBOTC Version 3.51 or newer"
#endif
// _Target_Emulator_ is new for 3.55, define for older versions
#if kRobotCVersionNumeric < 355
#if (_TARGET == "Emulator")
#ifndef _Target_Emulator_
#define _Target_Emulator_ 1
#endif
#endif
#endif
// Fix for versions of ROBOTC earlier than 4.26
#if kRobotCVersionNumeric < 426
#define tmotorVex393TurboSpeed_HBridge 9998
#define tmotorVex393TurboSpeed_MC29 9999
#endif
// ROBOTC with PID changed motor definitions - I used to use version number
// but 3.60 broke that and we don't know what version it will be now
// This will have to do for now until it's released after worlds
#ifdef bSmartMotorsWithEncoders
#define kRobotCHasPid
#endif
// System parameters - don't change
#define SMLIB_R_SYS 0.3
#define SMLIB_PWM_FREQ 1150
#define SMLIB_V_DIODE 0.75
// parameters for vex 393 motor
#define SMLIB_I_FREE_393 0.2
#define SMLIB_I_STALL_393 4.8
#define SMLIB_RPM_FREE_393 110
#define SMLIB_R_393 (7.2/SMLIB_I_STALL_393)
#define SMLIB_L_393 0.000650
#define SMLIB_Ke_393 (7.2*(1-SMLIB_I_FREE_393/SMLIB_I_STALL_393)/SMLIB_RPM_FREE_393)
#define SMLIB_I_SAFE393 0.90
// parameters for vex 269 motor
#define SMLIB_I_FREE_269 0.18
#define SMLIB_I_STALL_269 2.88
#define SMLIB_RPM_FREE_269 120
#define SMLIB_R_269 (7.2/SMLIB_I_STALL_269)
#define SMLIB_L_269 0.000650
#define SMLIB_Ke_269 (7.2*(1-SMLIB_I_FREE_269/SMLIB_I_STALL_269)/SMLIB_RPM_FREE_269)
#define SMLIB_I_SAFE269 0.75
// parameters for cortex and Power expander
// spec says 4A but we have set a little lower here
// may increase in a subsequent release.
#define SMLIB_I_SAFECORTEX 3.0
#define SMLIB_I_SAFEPE 3.0
// encoder counts per revolution depending on motor
#define SMLIB_TPR_269 240.448
#define SMLIB_TPR_393R 261.333
#define SMLIB_TPR_393S 392
#define SMLIB_TPR_393T 627.2
#define SMLIB_TPR_QUAD 360.0
#define SMLIB_TPR_POT 6000.0 // estimate
// Initial ambient temp 72deg F in deg C
#define SMLIB_TEMP_AMBIENT (( 72.0-32.0) * 5 / 9)
// Trip temperature for PTC 100 deg C, may be a little low
#define SMLIB_TEMP_TRIP 100.0
// Trip hysteresis in deg C, once tripped we need a 10 deg drop to enable normal operation
#define SMLIB_TEMP_HYST 10.0
// Reference temperature for data below, 25 deg C
#define SMLIB_TEMP_REF 25.0
// Hold current is the current where thr PTC should not trip
// Time to trip is the time at 5 x hold current
// K_TAU is a safety factor, probably 0.5 ~ 0.8
// we are being conservative here, it trip occurs too soon then increase
//
// PTC HR16-400 used in cortex and power expander
#define SMLIB_I_HOLD_CORTEX 3.0
#define SMLIB_T_TRIP_CORTEX 1.7
#define SMLIB_K_TAU_CORTEX 0.5
#define SMLIB_TAU_CORTEX (SMLIB_K_TAU_CORTEX * SMLIB_T_TRIP_CORTEX * 5.0 * 5.0)
#define SMLIB_C1_CORTEX ( (SMLIB_TEMP_TRIP - SMLIB_TEMP_REF) / (SMLIB_I_HOLD_CORTEX * SMLIB_I_HOLD_CORTEX) )
#define SMLIB_C2_CORTEX (1.0 / (SMLIB_TAU_CORTEX * 1000.0))
// PTC HR30-090 used in 393
//#define SMLIB_I_HOLD_393 0.9
#define SMLIB_I_HOLD_393 1.0 // increased a little to slow down trip
#define SMLIB_T_TRIP_393 7.1
#define SMLIB_K_TAU_393 0.5
#define SMLIB_TAU_393 (SMLIB_K_TAU_393 * SMLIB_T_TRIP_393 * 5.0 * 5.0)
#define SMLIB_C1_393 ( (SMLIB_TEMP_TRIP - SMLIB_TEMP_REF) / (SMLIB_I_HOLD_393 * SMLIB_I_HOLD_393) )
#define SMLIB_C2_393 (1.0 / (SMLIB_TAU_393 * 1000.0))
// PTC HR16-075 used in 269
#define SMLIB_I_HOLD_269 0.75
#define SMLIB_T_TRIP_269 2.0
#define SMLIB_K_TAU_269 0.5
#define SMLIB_TAU_269 (SMLIB_K_TAU_269 * SMLIB_T_TRIP_269 * 5.0 * 5.0)
#define SMLIB_C1_269 ( (SMLIB_TEMP_TRIP - SMLIB_TEMP_REF) / (SMLIB_I_HOLD_269 * SMLIB_I_HOLD_269) )
#define SMLIB_C2_269 (1.0 / (SMLIB_TAU_269 * 1000.0))
// No way to test 3 wire - Vamfun had them more or less the same as the 269
// PTC MINISMDC-075F used in three wire
#define SMLIB_C1_3WIRE SMLIB_C1_269
#define SMLIB_C2_3WIRE SMLIB_C2_269
// for forward reference
typedef struct smartController;
/*-----------------------------------------------------------------------------*/
/* */
/* This large structure holdes all the needed information for a single motor */
/* */
/* The constants used in calculations are all set automatically by the init */
/* code. Other variables are used by the different functions to calculate */
/* speed, current and temperature. some of these are stored for debug */
/* purposes */
/* */
/* V1.02 code uses 124 bytes (a couple are wasted due to word alignment) */
/* so 1240 bytes in all for the 10 motors. */
/*-----------------------------------------------------------------------------*/
typedef struct {
// the motor port
tMotor port;
// the motor encoder port
tMotor eport;
// copy of the system motor type
TMotorTypes type;
// due to alignment we get a free variable here
// use for debugging loop delay
short delayTimeMs;
// pointer to our control bank
smartController *bank;
// commanded speed comes from the user
// requested speed is either the commanded speed or limited speed if the PTC
// is about to trip
short motor_cmd;
short motor_req;
short motor_slew;
// current limit and max cmd value
short limit_tripped;
short limit_cmd;
float limit_current;
// the encoder associated with this motor
short encoder_id;
// encoder ticks per rev
float ticks_per_rev;
// variables used by rpm calculation
long enc;
long oldenc;
float delta;
float rpm;
// variables used for current calculation
float i_free;
float i_stall;
float r_motor;
float l_motor;
float ke_motor;
float rpm_free;
float v_bemf_max;
// instantaneous current
float current;
// a filtered version of current to remove some transients
float filtered_current;
// peak measured current
float peak_current;
// holds safe current for this motor
float safe_current;
// target current in limited mode
float target_current;
// PTC monitor variables
float temperature;
float t_const_1;
float t_const_2;
float t_ambient;
short ptc_tripped;
// Last program time we ran - may not keep this, bit overkill
long lastPgmTime;
} smartMotor;
// storage for all motors
static smartMotor sMotors[ kNumbOfRealMotors ];
// We have no inline so use a macro as shortcut to get ptr
#define _SmartMotorGetPtr( index ) ((smartMotor *)&sMotors[ index ])
/*-----------------------------------------------------------------------------*/
/* Motor control related definitions */
/*-----------------------------------------------------------------------------*/
#define SMLIB_MOTOR_MAX_CMD 127 // maximum command value to motor
#define SMLIB_MOTOR_MIN_CMD (-127) // minimum command value to motor
#define SMLIB_MOTOR_DEFAULT_SLEW_RATE 10 // Default will cause 375mS from full fwd to rev
#define SMLIB_MOTOR_FAST_SLEW_RATE 256 // essentially off
#define SMLIB_MOTOR_DEADBAND 10 // values below this are set to 0
// When current limit is not needed set limit_cmd to this value
#define SMLIB_MOTOR_MAX_CMD_UNDEFINED 255 // special value for limit_motor
// backards compatibility with last years library
static TSemaphore MotorSemaphore;
/*-----------------------------------------------------------------------------*/
/* */
/* This structure holds all the information for a controller bank */
/* which is a PTC protected circuit in the cortex or power expander */
/* */
/* We limit storage to three banks as that is the limit for a VEX competition */
/* robot although you could have more power expanders in theory */
/* */
/* V1.00 code uses 52 bytes per bank */
/*-----------------------------------------------------------------------------*/
// 3 banks of maximum 5 motors (even though power expander is 4)
#define SMLIB_TOTAL_NUM_CONTROL_BANKS 3
#define SMLIB_TOTAL_NUM_BANK_MOTORS 5
// Index for banks
#define SMLIB_CORTEX_PORT_0 0
#define SMLIB_CORTEX_PORT_1 1
#define SMLIB_PWREXP_PORT_0 2
typedef struct {
// array of pointers to the motors
smartMotor *motors[SMLIB_TOTAL_NUM_BANK_MOTORS];
// cumulative current
float current;
// peak measured current
float peak_current;
// holds safe current for this motor
float safe_current;
// PTC monitor variables
float temperature;
float t_const_1;
float t_const_2;
float t_ambient;
// flag for ptc status
short ptc_tripped;
// Do we have an led to show tripped status
tSensors statusLed;
// Power expander may have a status port
tSensors statusPort;
} smartController;
static smartController sPorts[SMLIB_TOTAL_NUM_CONTROL_BANKS];
// We have no inline so use a macro as shortcut to get ptr
#define _SmartMotorControllerGetPtr( index ) ((smartController *)&sPorts[ index ])
/*-----------------------------------------------------------------------------*/
/* Flags to determine behavior of the current limiting */
/*-----------------------------------------------------------------------------*/
// flag to hold global status to enable or disable the current limiter
// based on PTC temperature - defaults to off
static short PtcLimitEnabled = false;
// flag to hold global status to enable or disable the current limiter
// based on preset threshold - defaults to on
static short CurrentLimitEnabled = false;
/*-----------------------------------------------------------------------------*/
/* External debug function called once per loop with smartMotor ptr */
/*-----------------------------------------------------------------------------*/
#ifdef __SMARTMOTORLIBDEBUG__
void SmartMotorUserDebug( smartMotor *m );
#endif
/*******************************************************************************/
/*******************************************************************************/
/* PUBLIC FUNCTIONS */
/*******************************************************************************/
/*******************************************************************************/
/*-----------------------------------------------------------------------------*/
/* Get pointer to smartMotor structure - not used locally */
/* use the _SmartMotorGetPtr instead for functions in this file */
/*-----------------------------------------------------------------------------*/
smartMotor *
SmartMotorGetPtr( tMotor index )
{
// bounds check index
if((index < 0) || (index >= kNumbOfRealMotors))
return( NULL );
return( &sMotors[ index ] );
}
/*-----------------------------------------------------------------------------*/
/* Get pointer to smartController structure - not used locally */
/* use the _SmartMotorControllerGetPtr instead for functions in this file */
/*-----------------------------------------------------------------------------*/
smartController *
SmartMotorControllerGetPtr( short index )
{
// bounds check index
if((index < 0) || (index >= kNumbOfRealMotors))
return( NULL );
return( &sPorts[ index ] );
}
/*-----------------------------------------------------------------------------*/
/* Get Motor speed */
/*-----------------------------------------------------------------------------*/
float
SmartMotorGetSpeed( tMotor index )
{
// bounds check index
if((index < 0) || (index >= kNumbOfRealMotors))
return( 0 );
return( sMotors[ index ].rpm );
}
/*-----------------------------------------------------------------------------*/
/* Get Motor current */
/*-----------------------------------------------------------------------------*/
float
SmartMotorGetCurrent( tMotor index, int s = 0 )
{
// bounds check index
if((index < 0) || (index >= kNumbOfRealMotors))
return( 0 );
// normally return absolute current, s != 0 for signed
if(s)
return( sMotors[ index ].current );
else
return( abs( sMotors[ index ].current) );
}
/*-----------------------------------------------------------------------------*/
/* Get Motor temperature */
/*-----------------------------------------------------------------------------*/
float
SmartMotorGetTemperature( tMotor index )
{
// bounds check index
if((index < 0) || (index >= kNumbOfRealMotors))
return( 0 );
return( sMotors[ index ].temperature );
}
/*-----------------------------------------------------------------------------*/
/* Get Motor command limit */
/*-----------------------------------------------------------------------------*/
int
SmartMotorGetLimitCmd( tMotor index )
{
// bounds check index
if((index < 0) || (index >= kNumbOfRealMotors))
return( 0 );
return( sMotors[ index ].limit_cmd );
}
/*-----------------------------------------------------------------------------*/
/* Set Motor current limit */
/*-----------------------------------------------------------------------------*/
void
SmartMotorSetLimitCurent( tMotor index, float current = 1.0 )
{
// bounds check index
if((index < 0) || (index >= kNumbOfRealMotors))
return;
sMotors[ index ].limit_current = current;
}
/*-----------------------------------------------------------------------------*/
/* Set Motor maximum rpm */
/*-----------------------------------------------------------------------------*/
void
SmartMotorSetFreeRpm( tMotor index, short max_rpm )
{
// bounds check index
if((index < 0) || (index >= kNumbOfRealMotors))
return;
smartMotor *m = _SmartMotorGetPtr( index );
// set the max rpm for this motor
m->rpm_free = max_rpm;
// recalculate back emf constant
m->ke_motor = (7.2*(1-m->i_free/m->i_stall)/ m->rpm_free );
// recalculate maximum theoretical v_bemf
m->v_bemf_max = m->ke_motor * m->rpm_free;
}
/*-----------------------------------------------------------------------------*/
/* Set Motor slew rate */
/*-----------------------------------------------------------------------------*/
void
SmartMotorSetSlewRate( tMotor index, int slew_rate = 10 )
{
// bounds check index
if((index < 0) || (index >= kNumbOfRealMotors))
return;
// negative or 0 is invalid
if( slew_rate <= 0 )
return;
sMotors[ index ].motor_slew = slew_rate;
}
/*-----------------------------------------------------------------------------*/
/* Get Controller current */
/*-----------------------------------------------------------------------------*/
float
SmartMotorGetControllerCurrent( short index )
{
// bounds check index
if((index < 0) || (index >= SMLIB_TOTAL_NUM_CONTROL_BANKS))
return( 0 );
return( sPorts[ index ].current );
}
/*-----------------------------------------------------------------------------*/
/* Get Controller current */
/*-----------------------------------------------------------------------------*/
float
SmartMotorGetControllerTemperature( short index )
{
// bounds check index
if((index < 0) || (index >= SMLIB_TOTAL_NUM_CONTROL_BANKS))
return( 0 );
return( sPorts[ index ].temperature );
}
/*-----------------------------------------------------------------------------*/
/* Set Controller status LED */
/*-----------------------------------------------------------------------------*/
void
SmartMotorSetControllerStatusLed( int index, tSensors port )
{
// bounds check index
if((index < 0) || (index >= SMLIB_TOTAL_NUM_CONTROL_BANKS))
return;
// digital outputs have reversed logic so automatically change
if( SensorType[ port ] == sensorDigitalOut )
SensorType[ port ] = sensorLEDtoVCC;
// if an LED then use
if( SensorType[ port ] == sensorLEDtoVCC )
sPorts[ index ].statusLed = port;
}
/*-----------------------------------------------------------------------------*/
/* Set power expander status port */
/*-----------------------------------------------------------------------------*/
void
SmartMotorSetPowerExpanderStatusPort( tSensors port )
{
// if an Analog input then use
if( SensorType[ port ] == sensorAnalog )
sPorts[ SMLIB_PWREXP_PORT_0 ].statusPort = port;
}
/*-----------------------------------------------------------------------------*/
/* Enable current limit by monitoring the PTC temperatures */
/*-----------------------------------------------------------------------------*/
void
SmartMotorPtcMonitorEnable()
{
CurrentLimitEnabled = false;
PtcLimitEnabled = true;
}
/*-----------------------------------------------------------------------------*/
/* Disable current limit by monitoring the PTC temperatures */
/*-----------------------------------------------------------------------------*/
void
SmartMotorPtcMonitorDisable()
{
PtcLimitEnabled = false;
}
/*-----------------------------------------------------------------------------*/
/* Enable current limit by using a preset threshold */
/*-----------------------------------------------------------------------------*/
void
SmartMotorCurrentMonitorEnable()
{
PtcLimitEnabled = false;
CurrentLimitEnabled = true;
}
/*-----------------------------------------------------------------------------*/
/* Disable current limit by using a preset threshold */
/*-----------------------------------------------------------------------------*/
void
SmartMotorCurrentMonitorDisable()
{
CurrentLimitEnabled = false;
}
/*-----------------------------------------------------------------------------*/
/* After initialization the smart motor tasks need to be started */
/*-----------------------------------------------------------------------------*/
task SmartMotorTask();
task SmartMotorSlewRateTask();
void
SmartMotorRun()
{
// Higher priority than slew rate task
StartTask( SmartMotorTask , 100 );
// Higher priority than most user tasks
StartTask( SmartMotorSlewRateTask, 99 );
// Initialize resource semaphore
SemaphoreInitialize(MotorSemaphore);
}
/*-----------------------------------------------------------------------------*/
/* Stop smart motor tasks */
/*-----------------------------------------------------------------------------*/
void
SmartMotorStop()
{
SmartMotorPtcMonitorDisable();
SmartMotorCurrentMonitorDisable();
StopTask( SmartMotorTask );
StopTask( SmartMotorSlewRateTask );
}
/*-----------------------------------------------------------------------------*/
/* Dump controller and motor status to the debug stream */
/*-----------------------------------------------------------------------------*/
void
SmartMotorDebugStatus()
{
short i, j;
smartMotor *m;
smartController *s;
// Cortext ports 1 - 5
for(j=0;j<SMLIB_TOTAL_NUM_CONTROL_BANKS;j++)
{
s = _SmartMotorControllerGetPtr( j );
if( j == SMLIB_CORTEX_PORT_0 )
writeDebugStream("Cortex ports 1 - 5 - ");
if( j == SMLIB_CORTEX_PORT_1 )
writeDebugStream("Cortex ports 6 - 10 - ");
if( j == SMLIB_PWREXP_PORT_0 )
writeDebugStream("Power Expander - ");
writeDebugStream("Current:%5.2f ", s->current);
writeDebugStream("Temp:%6.2f ", s->temperature);
writeDebugStream("Status:%2d ", s->ptc_tripped);
writeDebugStreamLine("");
for(i=0;i<SMLIB_TOTAL_NUM_BANK_MOTORS;i++)
{
if( s->motors[i] != NULL )
{
m = s->motors[i];
writeDebugStream(" Motor Port: %d - ", m->port );
writeDebugStream("Current:%5.2f ", m->current);
writeDebugStream("Temp:%6.2f ", m->temperature);
writeDebugStream("Status:%2d ", m->ptc_tripped + (m->limit_tripped<<1) );
writeDebugStreamLine("");
}
}
writeDebugStreamLine("");
}
}
/*-----------------------------------------------------------------------------*/
/* New control value for the motor */
/* This is the only call that should be used to change motor speed */
/* using motor[] will completely bugger up the code - do not use it */
/* */
/* The naming of this function is inconsistent with the rest of the library */
/* to allow backwards compatibility with team 8888's existing library */
/*-----------------------------------------------------------------------------*/
void
SetMotor( int index, int value = 0, bool immeadiate = false )
{
smartMotor *m;
// bounds check index
if((index < 0) || (index >= kNumbOfRealMotors))
return;
// get motor
m = _SmartMotorGetPtr( index );
// limit value and set into motorReq
if( value > SMLIB_MOTOR_MAX_CMD )
m->motor_cmd = SMLIB_MOTOR_MAX_CMD;
else
if( value < SMLIB_MOTOR_MIN_CMD )
m->motor_cmd = SMLIB_MOTOR_MIN_CMD;
else
if( abs(value) >= SMLIB_MOTOR_DEADBAND )
m->motor_cmd = value;
else
m->motor_cmd = 0;
// new - for hard stop
if(immeadiate)
motor[ index ] = value;
}
/*-----------------------------------------------------------------------------*/
/* New users - do not use these calls, they are for backwards compatibity */
/* with the original team 8888 motorLib code */
/*-----------------------------------------------------------------------------*/
void
MotorLibInit()
{
SmartMotorStop();
StartTask( SmartMotorSlewRateTask );
// Initialize resource semaphore
SemaphoreInitialize(MotorSemaphore);
}
int
MotorGetSemaphore()
{
SemaphoreLock( MotorSemaphore, 2);
short s = getSemaphoreTaskOwner(MotorSemaphore);
if ( s == nCurrentTask )
return(1);
else
return(0);
}
void
MotorReleaseSemaphore()
{
short s = getSemaphoreTaskOwner(MotorSemaphore);
if ( s == nCurrentTask )
SemaphoreUnlock(MotorSemaphore);
}
/*-----------------------------------------------------------------------------*/
/* */
/* Initialize the smart motor library - This function must be called once */
/* when the program starts. Motors are automatically detected and assigned */
/* to the cortex banls, if a power expander is used then it should be added */
/* by using the SmartMotorsAddPowerExtender function after calling */
/* SmartMotorsInit */
/*-----------------------------------------------------------------------------*/
void
SmartMotorsInit()
{
int i, j;
smartMotor *m;
// clear controllers
for(j=0;j<SMLIB_TOTAL_NUM_CONTROL_BANKS;j++)
{
for(i=0;i<SMLIB_TOTAL_NUM_BANK_MOTORS;i++)
sPorts[j].motors[i] = NULL;
sPorts[j].t_const_1 = SMLIB_C1_CORTEX;
sPorts[j].t_const_2 = SMLIB_C2_CORTEX;
sPorts[j].t_ambient = SMLIB_TEMP_AMBIENT;
sPorts[j].temperature = sPorts[j].t_ambient;
sPorts[j].current = 0;
sPorts[j].peak_current = 0;
sPorts[j].safe_current = SMLIB_I_SAFECORTEX; // cortex and PE the same
sPorts[j].statusLed = (tSensors)(-1);
sPorts[j].statusPort = (tSensors)(-1);
}
// unfortunate kludge as getEncoderForMotor will not accept a variable yet
sMotors[0].encoder_id = getEncoderForMotor( port1 );
sMotors[1].encoder_id = getEncoderForMotor( port2 );
sMotors[2].encoder_id = getEncoderForMotor( port3 );
sMotors[3].encoder_id = getEncoderForMotor( port4 );
sMotors[4].encoder_id = getEncoderForMotor( port5 );
sMotors[5].encoder_id = getEncoderForMotor( port6 );
sMotors[6].encoder_id = getEncoderForMotor( port7 );
sMotors[7].encoder_id = getEncoderForMotor( port8 );
sMotors[8].encoder_id = getEncoderForMotor( port9 );
sMotors[9].encoder_id = getEncoderForMotor( port10 );
for(i=0;i<kNumbOfRealMotors;i++)
{
m = _SmartMotorGetPtr( i );
m->port = (tMotor)i;
m->eport = (tMotor)i;
m->type = (TMotorTypes)motorType[ m->port ];
switch( m->type )
{
// 393 set for high torque
#ifndef kRobotCHasPid
case tmotorVex393:
#else
case tmotorVex393_HBridge:
case tmotorVex393_MC29:
#endif
m->i_free = SMLIB_I_FREE_393;
m->i_stall = SMLIB_I_STALL_393;
m->r_motor = SMLIB_R_393;
m->l_motor = SMLIB_L_393;
m->ke_motor = SMLIB_Ke_393;
m->rpm_free = SMLIB_RPM_FREE_393;
m->ticks_per_rev = SMLIB_TPR_393T;
m->safe_current = SMLIB_I_SAFE393;
m->t_const_1 = SMLIB_C1_393;
m->t_const_2 = SMLIB_C2_393;
break;
// 393 set for high speed
#ifndef kRobotCHasPid
case tmotorVex393HighSpeed:
#else
case tmotorVex393HighSpeed_HBridge:
case tmotorVex393HighSpeed_MC29:
#endif
m->i_free = SMLIB_I_FREE_393;
m->i_stall = SMLIB_I_STALL_393;
m->r_motor = SMLIB_R_393;
m->l_motor = SMLIB_L_393;
m->ke_motor = SMLIB_Ke_393/1.6;
m->rpm_free = SMLIB_RPM_FREE_393 * 1.6;
m->ticks_per_rev = SMLIB_TPR_393S;
m->safe_current = SMLIB_I_SAFE393;
m->t_const_1 = SMLIB_C1_393;
m->t_const_2 = SMLIB_C2_393;
break;
// 393 set for Turbo
#ifndef kRobotCHasPid
// ROBOTC V3.XX has not been updated yet
// case tmotorVex393TurboSpeed:
#else
case tmotorVex393TurboSpeed_HBridge:
case tmotorVex393TurboSpeed_MC29:
#endif
m->i_free = SMLIB_I_FREE_393;
m->i_stall = SMLIB_I_STALL_393;
m->r_motor = SMLIB_R_393;
m->l_motor = SMLIB_L_393;
m->ke_motor = SMLIB_Ke_393/2.4;
m->rpm_free = SMLIB_RPM_FREE_393 * 2.4;
m->ticks_per_rev = SMLIB_TPR_393R;
m->safe_current = SMLIB_I_SAFE393;
m->t_const_1 = SMLIB_C1_393;
m->t_const_2 = SMLIB_C2_393;
break;
// 269 and 3wire set the same
#ifndef kRobotCHasPid
case tmotorVex269:
#else
case tmotorVex269_HBridge:
case tmotorVex269_MC29:
#endif
case tmotorServoContinuousRotation:
m->i_free = SMLIB_I_FREE_269;
m->i_stall = SMLIB_I_STALL_269;
m->r_motor = SMLIB_R_269;
m->l_motor = SMLIB_L_269;
m->ke_motor = SMLIB_Ke_269;
m->rpm_free = SMLIB_RPM_FREE_269;
m->ticks_per_rev = SMLIB_TPR_269;
m->safe_current = SMLIB_I_SAFE269;
m->t_const_1 = SMLIB_C1_269;
m->t_const_2 = SMLIB_C2_269;
break;
default:
// force OFF
// Servos, flashlights etc. not considered.
m->type = tmotorNone;
break;
}
// Override encoder ticks if not an IME
if( m->encoder_id < 0 )
{
// No encoder
m->ticks_per_rev = -1;
m->enc = 0;
m->oldenc = 0;
}
else
if( m->encoder_id < 20 ) {
// quad encoder
m->ticks_per_rev = SMLIB_TPR_QUAD;
m->enc = nMotorEncoder[ m->eport ];
m->oldenc = nMotorEncoder[ m->eport ];
}
else
{
m->enc = nMotorEncoder[ m->eport ];
m->oldenc = nMotorEncoder[ m->eport ];
}
// use until overidden by user
m->t_ambient = SMLIB_TEMP_AMBIENT;
m->temperature = m->t_ambient;
// default for target is safe
m->target_current = m->safe_current;
// default for limit is safe
m->limit_current = m->safe_current;
// reset stats
m->peak_current = 0;
// we are good to go
m->limit_tripped = false;
m->ptc_tripped = false;
m->limit_cmd = SMLIB_MOTOR_MAX_CMD_UNDEFINED;
// maximum theoretical v_bemf
m->v_bemf_max = m->ke_motor * m->rpm_free;
// add to controller