Skip to content

Commit 6734870

Browse files
PropGitPropGit
authored
Merge pull request #240 from VOCOM/master
Uploaded source files for tele-operated land-based locomotive
2 parents 087e999 + 53501e9 commit 6734870

File tree

8 files changed

+1199
-0
lines changed

8 files changed

+1199
-0
lines changed

‎libraries/community/p1/All/Tele-op Bot/CommControl.spin‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
{
2+
Project: EE-8 Practical 1
3+
Platform: Parallax Project USB Board
4+
Revision: 1
5+
Author: Muhammad Syamim
6+
Date: 17th Nov 2021
7+
Log:
8+
Date: Desc
9+
v1
10+
17/11/21: Added UART Communication OBJ using zigBee pins and baudrate
11+
v1.1
12+
22/11/21: Synced Milliseconds within OBJs
13+
v1.2
14+
24/11/21: Delegated new cog for op-code processing
15+
}
16+
CON
17+
_clkmode = xtal1 + pll16x 'Standard clock mode * crystal frequency = 80 MHz
18+
_xinfreq = 5_000_000
19+
20+
comRx = 20
21+
comTx = 21
22+
comBaud = 9600
23+
24+
comStart = $7A
25+
comForward = $01
26+
comReverse = $02
27+
comTurnLeft = $03
28+
comTurnRight = $04
29+
comStopAll = $AA
30+
31+
VAR
32+
long _Ms_001
33+
long CommCogID, CommCogStack[64]
34+
35+
OBJ
36+
Comm : "FullDuplexSerial.spin" 'UART Communication for control
37+
38+
PUB Init(DirPtr , RDYPtr, MsVal) 'Initialise Core for Communications
39+
40+
_Ms_001 := MsVal 'Sync time delays
41+
StopCore 'Prevent stacking drivers
42+
CommCogID := cognew(Start(DirPtr, RDYPtr), @CommCogStack) 'Initialise new cog with Start method
43+
44+
return CommCogID
45+
46+
PUB Start(DirPtr, RDYPtr) | rxVal 'Looping code for Op-Code update
47+
48+
Comm.Start(comRx, comTx, 0, comBaud) 'Start new cog for UART Communication with ZigBee
49+
BYTE[RDYPtr]++ 'Update Ready Byte
50+
51+
repeat
52+
rxVal := Comm.rx 'Wait until incomming BYTE
53+
if rxVal == comStart 'Protocol starts with start BYTE
54+
comm.dec(rxval)
55+
rxVal := Comm.rx 'Retrieve direction BYTE
56+
case rxVal 'Update direction using Op-Code
57+
comForward:
58+
BYTE[DirPtr] := 1
59+
comReverse:
60+
BYTE[DirPtr] := 2
61+
comTurnRight:
62+
BYTE[DirPtr] := 3
63+
comTurnLeft:
64+
BYTE[DirPtr] := 4
65+
comStopAll:
66+
BYTE[DirPtr] := 0
67+
68+
PUB StopCore 'Stop active cog
69+
if CommCogID 'Check for active cog
70+
cogStop(CommCogID~) 'Stop the cog and zero out ID
71+
return CommCogID
72+
73+
PRI Pause(ms) | t
74+
t := cnt - 1088
75+
repeat (ms #> 0)
76+
waitcnt(t += _Ms_001)
77+
return

‎libraries/community/p1/All/Tele-op Bot/MotorControl.spin‎

Lines changed: 148 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,148 @@
1+
{
2+
Project: EE-6 Practical 1
3+
Platform: Parallax Project USB Board
4+
Revision: 1.3
5+
Author: Muhammad Syamim
6+
Date: 14th Nov 2021
7+
Log:
8+
Date: Desc
9+
05/11/21: Added RoboClaw and Serial OBJ
10+
Motor control success
11+
v1.1
12+
07/11/21: Added new cog initialisation
13+
Added track selection
14+
v1.2
15+
14/11/21: Added stop flag pointer
16+
17+
v1.3
18+
16/11/21: Added Post-Clear operator for MotorCogID
19+
Offloaded Directional control to cog 0
20+
Synced Milliseconds within OBJs
21+
v1.4
22+
26/11/21: Offloaded Speed to cog 0
23+
}
24+
CON
25+
_clkmode = xtal1 + pll16x 'Standard clock mode * crystal frequency = 80 MHz
26+
_xinfreq = 5_000_000
27+
28+
M1 = 10, M2 = 11, M3 = 12, M4 = 13 'Motor Pins
29+
30+
M1_zero = 1520 'RoboClaw initial pulse width for zero velocity
31+
M2_zero = 1520 '1120 = Full Reverse
32+
M3_zero = 1520 '1520 = Full Stop
33+
M4_zero = 1520 '1920 = Full Forward
34+
35+
'Macros
36+
'SPD = 20 'Speed setting from 0% to 100%
37+
DST = 1000 'Distance in milliseconds
38+
ANG = 1000 'Angle in milliseconds
39+
40+
VAR
41+
long MotorCogID, MotorCogStack[64]
42+
long _Ms_001
43+
44+
OBJ
45+
MDriver : "Servo8Fast_vZ2.spin" '1120 Full reverse, 1520 stop, 1920 Full Forward
46+
47+
PUB Start(StopFlagPtr, DirFlagPtr, FSpdPtr, RSpdPtr, RDYPtr) | i 'Track Selection
48+
MDriver.Init 'Initialise RoboClaw encoder pins
49+
MDriver.AddSlowPin(M1)
50+
MDriver.AddSlowPin(M2)
51+
MDriver.AddSlowPin(M3)
52+
MDriver.AddSlowPin(M4)
53+
MDriver.Start
54+
Pause(1000) 'Delay for RoboClaw calibration procedure
55+
StopAllMotors 'Calibrate RoboClaw to new zero point value
56+
Pause(1000)
57+
BYTE[RDYPtr]++ 'Update Ready BYTE
58+
repeat until BYTE[StopFlagPtr] 'Check for EStop
59+
case BYTE[DirFlagPtr] 'Check with cog 0 for directions
60+
0:
61+
StopAllMotors
62+
2:
63+
if NOT BYTE[StopFlagPtr + 1] 'Check for obstacle in front
64+
Forward(BYTE[FSpdPtr])
65+
else
66+
StopAllMotors
67+
1:
68+
if NOT BYTE[StopFlagPtr + 2] 'Check for obstacle in rear
69+
Reverse(BYTE[RSpdPtr])
70+
else
71+
StopAllMotors
72+
3:
73+
TurnRight((BYTE[FSpdPtr] + BYTE[RSpdPtr]) / 2)
74+
4:
75+
TurnLeft((BYTE[FSpdPtr] + BYTE[RSpdPtr]) / 2)
76+
77+
PUB Init(StopFlagPtr, DirFlagPtr, FSpdPtr, RSpdPtr, RDYPtr, MsVal) 'Initialise Core for motor control
78+
_Ms_001 := MsVal 'Sync time delays
79+
StopCore 'Prevent stacking drivers
80+
MotorCogID := cognew(Start(StopFlagPtr, DirFlagPtr, FSpdPtr, RSpdPtr, RDYPtr), @MotorCogStack) 'Start new cog with Start method
81+
return MotorCogID 'Return cogID for tracking
82+
83+
PUB StopCore 'Stop active cog
84+
if MotorCogID 'Check for active cog
85+
cogstop(MotorCogID~) 'Stop the cog and zero out ID
86+
return MotorCogID
87+
88+
PUB Set(motor, speed) 'Set the speed of selected motor
89+
90+
speed *= 4 'Convert speed into value within range
91+
92+
case motor 'Select motor & set the speed with respect to zero point
93+
1:
94+
MDriver.Set(M1, M1_zero + speed)
95+
2:
96+
MDriver.Set(M2, M2_zero + speed)
97+
3:
98+
MDriver.Set(M3, M3_zero + speed)
99+
4:
100+
MDriver.Set(M4, M4_zero + speed)
101+
102+
return
103+
104+
PUB StopAllMotors | i 'Set all motors to zero point
105+
106+
repeat i from 1 to 4 'Cycle through all the motors
107+
Set(i,0) 'Set the motor to zero point in %
108+
109+
return
110+
111+
PUB Forward(speed) | i 'Set motors to forward direction
112+
113+
repeat i from 0 to 4 'Cycle through all the motors
114+
Set(i, speed) 'Set the motor to the speed
115+
116+
return
117+
118+
PUB Reverse(speed) | i 'Set motors to reverse direction
119+
120+
repeat i from 1 to 4 'Cycle through all the motors
121+
Set(i, -speed) 'Set the motor to the speed
122+
123+
return
124+
125+
PUB TurnRight(speed) 'Set motors to turn right
126+
127+
Set(1, -speed) 'Pivot point on centre of machine
128+
Set(2, speed)
129+
Set(3, -speed)
130+
Set(4, speed)
131+
132+
return
133+
134+
PUB TurnLeft(speed) 'Set motors to turn left
135+
136+
Set(1, speed) 'Pivot point on centre of machine
137+
Set(2, -speed)
138+
Set(3, speed)
139+
Set(4, -speed)
140+
141+
return
142+
143+
PRI Pause(ms) | t
144+
t := cnt - 1088
145+
repeat (ms #> 0)
146+
waitcnt(t += _Ms_001)
147+
148+
return

‎libraries/community/p1/All/Tele-op Bot/MyLiteKit.spin‎

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
{
2+
Project: EE-9 Practical 1
3+
Platform: Parallax Project USB Board
4+
Revision: 1.1
5+
Author: Muhammad Syamim
6+
Date: 14th Nov 2021
7+
Log:
8+
Date: Desc
9+
v1
10+
14/11/21: Added Cog bootstrapping & stop flag
11+
12+
v1.1
13+
16/11/21: Offloaded conditional checks and direction control to cog 0
14+
for futureproof modularity
15+
v1.2
16+
17/11/21: Added delay for bootstrapping
17+
Added ZigBee driver
18+
v1.3
19+
23/11/21: Streamlined operation directive & conditional checks
20+
Stop flag to array to accomodate directional stop
21+
v1.4
22+
24/11/21: Adjusted Ultrasonic Sensor range to account for echo loss
23+
v1.5
24+
26/11/21: Added Speed setting
25+
=========================================================== | Delays to note:
26+
| Cog 0 | Cog 1 | Cog 2 | Cog 3 | Cog 4 | | Motor Driver Boot time: 2s
27+
| Behaviour | Sensor | Motor | Op-code | ZigBee UART | | Communication Driver Boot time: 1s
28+
| Control | Polling | Control | Sync | Communication | | Ultrasonic Sensor Poll rate: 40ms / Sensor
29+
| | | | | | |
30+
=========================================================== |
31+
}
32+
CON
33+
_clkmode = xtal1 + pll16x 'Standard clock mode * crystal frequency = 80 MHz
34+
_xinfreq = 5_000_000
35+
_ConClkFreq = ((_clkmode - xtal1) >> 6) * _xinfreq
36+
_Ms_001 = _ConClkFreq / 1_000
37+
38+
ToFStop = 230
39+
40+
VAR
41+
42+
long MCID, SCID, CCID 'cogID for tracking
43+
BYTE RDY, Dir, FSpd, RSpd 'Ready, Direction, Forward & Reverse speed flag
44+
long F_Ult, R_Ult, F_ToF, R_ToF 'Storage for sensor values
45+
BYTE Stop[3] 'Stop byte array | 0 = EStop, 1 = Front Stop, 2 = Rear Stop
46+
47+
OBJ
48+
49+
'Term : "FullDuplexSerial.spin" 'UART communication
50+
Sensor : "SensorControl.spin" 'Ultrasonic and ToF Sensors
51+
Motor : "MotorControl.spin" 'RoboClaw Motor Driver
52+
Comm : "CommControl.spin" 'ZigBee Driver
53+
54+
PUB Main 'Lite Kit Semi-Autonomous Land based Tele-operated Vehicle
55+
'Initilisation
56+
SCID := Sensor.Init(@F_Ult, @R_Ult, @F_ToF, @R_ToF, @RDY, _Ms_001) 'Initialise Sensor Driver
57+
MCID := Motor.Init(@Stop, @Dir, @FSpd, @RSpd, @RDY, _Ms_001) 'Initialise Motor Driver
58+
CCID := Comm.Init(@Dir, @RDY, _Ms_001) 'Initialise Control Driver
59+
repeat while RDY < 3 'Wait for bootstrapping
60+
Pause(100) 'Account for intital sensor poll
61+
FSpd := 30
62+
RSpd := 30
63+
repeat until Stop[0] 'Update instructions | 100ms polling time
64+
'EStop 'Emergency Condition
65+
ObjDetect 'Obstacle condition
66+
'SetSpd 'Update Speed Setting
67+
MCID := Motor.StopCore 'Disengage Motor cog
68+
Motor.StopAllMotors 'Ensure all motor stopped
69+
70+
repeat 'Suspend cog 0
71+
72+
PRI SetSpd 'Speed Control
73+
74+
if F_Ult > 1500 OR F_Ult == 0 'Check special case (debouncing yet to be implemented)
75+
FSpd := 75 'Double instance of 0, > 2m & Exact 0m
76+
elseif F_Ult > 1000
77+
FSpd := 50
78+
else
79+
FSpd := 25
80+
if R_Ult > 1500 OR R_Ult == 0 'Check special case (debouncing yet to be implemented)
81+
RSpd := 75 'Double instance of 0, > 2m & Exact 0m
82+
elseif R_Ult > 1000
83+
RSpd := 50
84+
else
85+
RSpd := 25
86+
87+
return
88+
89+
PRI ObjDetect 'Object Detection
90+
91+
if (F_Ult > 0 AND F_Ult < 300) OR F_ToF > ToFStop 'Front Object and Ditch condition
92+
Stop[1] := TRUE
93+
else
94+
Stop[1] := FALSE
95+
96+
if (R_Ult > 0 AND R_Ult < 300) OR R_ToF > ToFStop 'Rear Object and Ditch condition
97+
Stop[2] := TRUE
98+
else
99+
Stop[2] := FALSE
100+
101+
return
102+
103+
PRI EStop 'Emergency stop condition check (Not in use)
104+
if F_Ult < 200 OR R_Ult < 200' OR F_ToF > 250 OR R_ToF > 250
105+
Stop[0] := TRUE
106+
107+
return
108+
109+
PRI Pause(ms) | t
110+
t := cnt - 1088
111+
repeat (ms #> 0)
112+
waitcnt(t += _Ms_001)
113+
114+
return

0 commit comments

Comments
 (0)