This repository has been archived by the owner on Aug 13, 2021. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Speed Controller Calibrate.bsx
429 lines (347 loc) · 17.6 KB
/
Speed Controller Calibrate.bsx
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
' PROGRAM: Full-Size RC 2003 Default (2 Joystick Drive)
' Written by: Innovation First, Inc.
' Date: 2002 Dec 18
'
' Define BS2-SX Project Files
'
' {$PBASIC 2.5}
' {$STAMP BS2SX}
'=============================================================================================================
'========== DECLARE VARIABLES ================================================================================
'=============================================================================================================
' Below is a list of declared input and output variables. Comment or un-comment
' the variables as needed. Declare any additional variables required in
' your main program loop. Note that you may only use 26 total variables.
'---------- Operator Interface (OI) - Analog Inputs ----------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
p2_x VAR byte 'Port 2, X-axis on Joystick
p4_x VAR byte 'Port 4, X-axis on Joystick
p2_y VAR byte 'Port 2, Y-axis on Joystick
p4_y VAR byte 'Port 4, Y-axis on Joystick
'---------- Operator Interface - Digital Inputs --------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
oi_swA VAR byte 'OI Digital Switch Inputs 1 thru 8
oi_swB VAR byte 'OI Digital Switch Inputs 9 thru 16
'---------- Robot Controller - Digital Inputs ----------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
rc_swA VAR byte 'RC Digital Inputs 1 thru 8
rc_swB VAR byte 'RC Digital Inputs 9 thru 16
'---------- Robot Controller - Digital Outputs ---------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
relayA VAR byte
relayB VAR byte
'---------- Misc. --------------------------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
packet_num VAR byte
'delta_t VAR byte
PB_mode VAR byte
pwm1 VAR byte
pwm2 VAR byte
pwm3 VAR byte
pwm4 VAR byte
pwm5 VAR byte
pwm6 VAR byte
pwm7 VAR byte
pwm8 VAR byte
pwm9 VAR byte
pwm10 VAR byte
pwm11 VAR byte
pwm12 VAR byte
'=============================================================================================================
'========== DEFINE ALIASES ===================================================================================
'=============================================================================================================
' Aliases are variables which are sub-divisions of variables defined
' above. Aliases don't require any additional RAM.
'---------- Aliases for each OI switch input -----------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' Below are aliases for the digital inputs located on the Operator Interface.
' Ports 1 & 3 have their inputs duplicated in ports 4 & 2 respectively. The
' inputs from ports 1 & 3 may be disabled via the 'Disable' dip switch
' located on the Operator Interface. See Users Manual for details.
p1_sw_trig VAR oi_swA.bit0 'Joystick Trigger Button, same as Port4 pin5
p1_sw_top VAR oi_swA.bit1 'Joystick Top Button, same as Port4 pin8
p1_sw_aux1 VAR oi_swA.bit2 'Aux input, same as Port4 pin9
p1_sw_aux2 VAR oi_swA.bit3 'Aux input, same as Port4 pin15
p3_sw_trig VAR oi_swA.bit4 'Joystick Trigger Button, same as Port2 pin5
p3_sw_top VAR oi_swA.bit5 'Joystick Top Button, same as Port2 pin8
p3_sw_aux1 VAR oi_swA.bit6 'Aux input, same as Port2 pin9
p3_sw_aux2 VAR oi_swA.bit7 'Aux input, same as Port2 pin15
p2_sw_trig VAR oi_swB.bit0 'Joystick Trigger Button
p2_sw_top VAR oi_swB.bit1 'Joystick Top Button
p2_sw_aux1 VAR oi_swB.bit2 'Aux input
p2_sw_aux2 VAR oi_swB.bit3 'Aux input
p4_sw_trig VAR oi_swB.bit4 'Joystick Trigger Button
p4_sw_top VAR oi_swB.bit5 'Joystick Top Button
p4_sw_aux1 VAR oi_swB.bit6 'Aux input
p4_sw_aux2 VAR oi_swB.bit7 'Aux input
'---------- Aliases for each RC switch input -----------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' Below are aliases for the digital inputs located on the Robot Controller.
rc_sw1 VAR rc_swA.bit0
rc_sw2 VAR rc_swA.bit1
rc_sw3 VAR rc_swA.bit2
rc_sw4 VAR rc_swA.bit3
rc_sw5 VAR rc_swA.bit4
rc_sw6 VAR rc_swA.bit5
rc_sw7 VAR rc_swA.bit6
rc_sw8 VAR rc_swA.bit7
rc_sw9 VAR rc_swB.bit0
rc_sw10 VAR rc_swB.bit1
rc_sw11 VAR rc_swB.bit2
rc_sw12 VAR rc_swB.bit3
rc_sw13 VAR rc_swB.bit4
rc_sw14 VAR rc_swB.bit5
rc_sw15 VAR rc_swB.bit6
rc_sw16 VAR rc_swB.bit7
'---------- Aliases for each RC Relay outputs ----------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' Below are aliases for the relay outputs located on the Robot Controller.
relay1_fwd VAR RelayA.bit0
relay1_rev VAR RelayA.bit1
relay2_fwd VAR RelayA.bit2
relay2_rev VAR RelayA.bit3
relay3_fwd VAR RelayA.bit4
relay3_rev VAR RelayA.bit5
relay4_fwd VAR RelayA.bit6
relay4_rev VAR RelayA.bit7
relay5_fwd VAR RelayB.bit0
relay5_rev VAR RelayB.bit1
relay6_fwd VAR RelayB.bit2
relay6_rev VAR RelayB.bit3
relay7_fwd VAR RelayB.bit4
relay7_rev VAR RelayB.bit5
relay8_fwd VAR RelayB.bit6
relay8_rev VAR RelayB.bit7
'---------- Aliases for the Pbasic Mode Byte (PB_mode) -------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' Bit 7 of the PB_mode byte (aliased as comp_mode below) indicates the status
' of the Competition Control, either Enabled or Disabled. This indicates the
' starting and stopping of rounds at the competitions.
' Comp_mode is indicated by a solid "Disabled" LED on the Operator Interface.
' Comp_mode = 1 for Enabled, 0 for Disabled.
'
' Bit 6 of the PB_mode byte (aliased as auton_mode below) indicates the status
' of the Autonomous Mode, either Autonomous or Normal. This indicates when
' the robot must run on its own programming. When in Autonomous Mode, all
' OI analog inputs are set to 127 and all OI switch inputs are set to 0 (zero).
' Auton_mode is indicated by a blinking "Disabled" LED on the Operator Interface.
' Auton_mode = 1 for Autonomous, 0 for Normal.
'
' Autonomous Mode can be turned ON by setting the RC to Team 0 (zero).
'
' Bit 5 of the PB_mode byte (aliased as user_display_mode below) indicates when
' the user selects the "User Mode" on the OI. PB_mode.bit5 is set to 1 in "User Mode".
' When the user selects channel, team number, or voltage, PB_mode.bit5 is set to 0
' When in "User Mode", the eight Robot Feedback LED are turned OFF.
' Note: "User Mode" is identified by the letter u in the left digit (for 4 digit OI's)
' Note: "User Mode" is identified by decimal places on the right two digits (for 3 digit OI's)
comp_mode VAR PB_mode.bit7
auton_mode VAR PB_mode.bit6
user_display_mode VAR PB_mode.bit5
'=============================================================================================================
'========= DEFINE CONSTANTS FOR INITIALIZATION ===============================================================
'=============================================================================================================
' The initialization code is used to select the input data used by PBASIC.
' The Master micro-processor (uP) sends the data you select to the BS2SX
' PBASIC uP. You may select up to 26 constants, corresponding
' to 26 variables, from the 32 available to you. Make sure that you have
' variables for all the bytes recieved in the serin command.
'
' The constants below have a "c_" prefix, as compared to the variables that
' they will represent.
'
' Set the Constants below to 1 for each data byte you want to recieve.
' Set the Constants below to 0 for the unneeded data bytes.
'---------- Set the Initialization constants you want to read ------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
c_p1_y CON 0
c_p2_y CON 1
c_p3_y CON 0
c_p4_y CON 1
c_p1_x CON 0
c_p2_x CON 1
c_p3_x CON 0
c_p4_x CON 1
c_p1_wheel CON 0
c_p2_wheel CON 0
c_p3_wheel CON 0
c_p4_wheel CON 0
c_p1_aux CON 0
c_p2_aux CON 0
c_p3_aux CON 0
c_p4_aux CON 0
c_oi_swA CON 1
c_oi_swB CON 1
c_sensor1 CON 0
c_sensor2 CON 0
c_sensor3 CON 0
c_sensor4 CON 0
c_sensor5 CON 0
c_sensor6 CON 0
c_sensor7 CON 0
c_batt_volt CON 0
c_rc_swA CON 1
c_rc_swB CON 1
c_delta_t CON 0
c_PB_mode CON 1
c_packet_num CON 1
c_res01 CON 0
'---------- Initialization Constant VOLTAGE - USER DEFINED ---------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' This is the 'Low Battery' detect voltage. The 'Low Battery' LED will
' blink when the voltage drops below this value.
' Basically, the value = ((DESIRED FLASH VOLTAGE * 16.46) - 8.35)
' Example, for a 6.5 Volt Flash trigger, set value = 99.
dataInitVolt CON 140
'=============================================================================================================
'========== DEFINE CONSTANTS (DO NOT CHANGE) =================================================================
'=============================================================================================================
' Baud rate for communications with User CPU
OUTBAUD CON 20 '(62500, 8N1, Noninverted)
INBAUD CON 20 '(62500, 8N1, Noninverted)
USERCPU CON 4
FPIN CON 1
COMA CON 1
COMB CON 2
COMC CON 3
'=============================================================================================================
'========== MAIN PROGRAM =====================================================================================
'=============================================================================================================
'---------- Input & Output Declarations ----------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
Output COMB
Input COMA
Input COMC
Output 7 'define Basic Run LED on RC => out7
Output 8 'define Robot Feedback LED => out8 => PWM1 Green
Output 9 'define Robot Feedback LED => out9 => PWM1 Red
Output 10 'define Robot Feedback LED => out10 => PWM2 Green
Output 11 'define Robot Feedback LED => out11 => PWM2 Red
Output 12 'define Robot Feedback LED => out12 => Relay1 Red
Output 13 'define Robot Feedback LED => out13 => Relay1 Green
Output 14 'define Robot Feedback LED => out14 => Relay2 Red
Output 15 'define Robot Feedback LED => out15 => Relay2 Green
'---------- Initialize Inputs & Outputs ----------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
Out7 = 1 'Basic Run LED on RC
Out8 = 0 'PWM1 LED - Green
Out9 = 0 'PWM1 LED - Red
Out10 = 0 'PWM2 LED - Green
Out11 = 0 'PWM2 LED - Red
Out12 = 0 'Relay1 LED - Red
Out13 = 0 'Relay1 LED - Green
Out14 = 0 'Relay2 LED - Red
Out15 = 0 'Relay2 LED - Green
'=============================================================================================================
'========== PBASIC - MASTER uP INITIALIZATION ROUTINE ========================================================
'=============================================================================================================
' DO NOT CHANGE THIS! DO NOT MOVE THIS!
' The init routine sends 5 bytes to the Master uP, defining which data bytes to receive.
' 1) Collect init.
' 2) Lower the COMA line, which is the clk line for the shift out command.
' 3) Lower COMB line to tell pic that we are ready to send init data.
' 4) Wait for pic to lower the COMC line, signaling pic is ready for data.
' 5) Now send out init dat to pic, all 5 bytes.
' 6) Now set direction and levels for the COMA and COMB pins.
tempA CON c_p3_x <<1 + c_p4_x <<1 + c_p1_x <<1 + c_p2_x <<1 + c_rc_swB
dataInitA CON tempA <<1 + c_rc_swA <<1 + c_oi_swB <<1 + c_oi_swA
tempB CON c_sensor4 <<1 + c_sensor3 <<1 + c_p1_y <<1 + c_p2_y <<1 + c_sensor2
dataInitB CON tempB <<1 + c_sensor1 <<1 + c_packet_num <<1 + c_PB_mode
tempC CON c_batt_volt <<1 + c_sensor7 <<1 + c_p1_wheel <<1 + c_p2_wheel <<1 + c_sensor6
dataInitC CON tempC <<1 + c_sensor5 <<1 + c_p3_y <<1 + c_p4_y
tempD CON c_res01 <<1 + c_delta_t <<1 + c_p3_aux <<1 + c_p4_aux <<1 + c_p1_aux
dataInitD CON tempD <<1 + c_p2_aux <<1 + c_p3_wheel <<1 + c_p4_wheel
Output COMA
low COMA
low COMB
Input COMC
Wait_init: if IN3 = 1 then Wait_init:
Shiftout COMB,COMA,1, [dataInitA,dataInitB,dataInitC,dataInitD,dataInitVolt]
Input COMA
high COMB
Output COMC
low COMC
'=============================================================================================================
'========== MAIN DO LOOP =====================================================================================
'=============================================================================================================
Do
'---------- Serin Command - Get Data from Master uP ----------------------------------------------------------
' Construct the "serin" command using the following rules:
' 1) There must be one variable for every input defined in the "Define Constants for Init" section.
' 2) The order must match the order in the EXAMPLE SERIN COMMAND below.
' 3) The total number of all variables may not exceed 26.
' 4) Only use one "Serin" command.
' 5) The Serin command must occupy one line.
'
' If you see a BASIC INIT ERR on the Robot Controller after programming and pressing RESET, then
' there is a problem with the Serin command below. Check the number of variables. A BASIC INIT ERR
' will not occur if you have the variables in the wrong order, however your code will not work correctly.
'
' EXAMPLE SERIN COMMAND
' This example exceed the 26 variable limit and is not on one line:
'
' Serin COMA\COMB, INBAUD, [oi_swA,oi_swB,rc_swA,rc_swB,p2_x,p1_x,p4_x,p3_x,PB_mode,packet_num,sensor1,
' sensor2,p2_y,p1_y,sensor3,sensor4,p4_y,p3_y,sensor5,sensor6,p2_wheel,p1_wheel,
' sensor7,sensor8,p4_wheel,p3_wheel,p2_aux,p1_aux,p4_aux,p3_aux,delta_t,res01]
'
Serin COMA\COMB, INBAUD, [oi_swA,oi_swB,rc_swA,rc_swB,p2_x,p4_x,PB_mode,packet_num,
p2_y,p4_y]
'---------- Blink BASIC RUN LED ------------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
Toggle 7 'Basic Run LED on the RC is toggled ON/OFF every loop.
'=============================================================================================================
'========== PERFORM OPERATIONS ===============================================================================
'=============================================================================================================
' Mappings for 2003 outputs
'**************************************************************************************************************
'* color table of relays
'* 1 none air pump
'* 2 red/black front skid plate
'* 3 purple/black back skid plate
'* 4 brown/black stacker hook
'* 5 orange/black
'* 6 yellow/black
'* 7 grey/black
'* 8 blue/black light
'*
'* color table of pwms
'* 1 none
'* 2 red front left drive
'* 3 purple right crab
'* 4 brown left crab
'* 5 orange front right drive
'* 6 yellow front wing
'* 7 grey back wing
'* 8 green back left drive
'* 9 blue back right drive
'* 10 white stacker
'* 11 black
'* 12 ?????
'**************************************************************************************************************
'to calibrate speed controller, set pwm<x> to joystick input (ie p2_x, p2_y, p4_x, p4_y)
'debug ?p4_y
pwm1 = 127
pwm2 = 127
pwm3 = 127
pwm4 = 127
pwm5 = 127
pwm6 = 127
pwm7 = 127
pwm8 = 127
pwm9 = 127
pwm10 = 127
pwm11 = 127
pwm12 = 127
'=============================================================================================================
'========== OUTPUT DATA ======================================================================================
'=============================================================================================================
' The Serout line sends data to the Output uP. The Output uP passes this to each PWM 1-16
' and Relay 1-8. The Output uP will not output data if there is no communication with the
' Operator Interface or if the Competition Mode is Disabled. Do not delete any elements
' from the Serout array. Set unused PWM outputs to 127. Set unused relay outputs to 0.
'
' Serout USERCPU, OUTBAUD, [255,255,(PWM1),relayA,(PWM2),relayB,(PWM3),(PWM4),(PWM5),(PWM6),(PWM7),(PWM8),
' (PWM9),(PWM10),(PWM11),(PWM12),(PWM13),(PWM14),(PWM15),(PWM16)]
Serout USERCPU, OUTBAUD, [255,255,pwm1,0,pwm2,0,pwm3,pwm4,pwm5,pwm6,pwm7,pwm8,
pwm9,pwm10,pwm11,pwm12,127,127,127,127]
Loop