00001 function dy = trucksim(t,y) 00002 % y(1) = x 00003 % y(2) = y 00004 % y(3) = theta (angle of truck body) 00005 % y(4) = steering angle (actual) 00006 % y(5) = velocity of truck 00007 % y(6) = travel of right wheels (in meters) 00008 % y(7) = travel of left wheels (in meters) 00009 % 00010 % use y(7) and y(8) as control inputs 00011 % they will be kept constant for duration 00012 % of the simulation 00013 % y(7) = speed controller power (input) 00014 % y(8) = target steering angle (input) 00015 00016 00017 % length of truck (in meters) from back axle to front axle 00018 truckLength = 0.3048; 00019 00020 % length from center of right wheel to left wheel of truck 00021 axleLength = 0.37465; 00022 00023 % number of encoder ticks per meter of wheel travel 00024 ticksPerMeter = 1183; 00025 00026 00027 % use names for some of the state values to make the 00028 % formula's easier to follow 00029 truckAngle = y(3); 00030 steeringAngle = y(4); 00031 truckVelocity = y(5); 00032 scPower = y(7); 00033 targetSteeringAngle = y(8); 00034 00035 dy = zeros(8,1); 00036 00037 % dynamic equations 00038 dy(1) = truckVelocity * cos(truckAngle); 00039 dy(2) = truckVelocity * sin(truckAngle); 00040 dy(3) = tan(steeringAngle) / truckLength; 00041 00042 % both these formula's are guesses and need to be determined 00043 dy(4) = 2.0 * (steeringAngle - targetSteeringAngle) * truckVelocity; 00044 dy(5) = 2.0 * (truckVelocity - scPower * ) 00045 00046 % these values are in meters 00047 dy(6) = truckVelocity + 0.5 * axleLength * dy(3); 00048 dy(7) = truckVelocity - 0.5 * axleLength * dy(3);
1.4.4