PID
Repository
https://github.com/Willie169/PIDWorkflow
Design kart:
import FreeCAD, Part, Draft
from FreeCAD import Base
def create_wheel(radius, width, position):
wheel = Part.makeCylinder(radius, width)
wheel.translate(Base.Vector(position[0], position[1], position[2]))
return wheel
def create_kart_body(length, width, height):
body = Part.makeBox(length, width, height)
return body
def create_kart_frame(length, width, height):
frame = Part.makeBox(length, width, height)
frame.translate(Base.Vector(0, 0, height / 2)) return frame
def create_arduino(length, width, height):
arduino = Part.makeBox(length, width, height)
arduino.translate(Base.Vector(10, 10, 0)) return arduino
def create_ultrasonic_sensor(length, width, height, position):
sensor = Part.makeBox(length, width, height)
sensor.translate(Base.Vector(position[0], position[1], position[2]))
return sensor
def create_motor_controller(length, width, height, position):
motor_controller = Part.makeBox(length, width, height)
motor_controller.translate(Base.Vector(position[0], position[1], position[2]))
return motor_controller
def create_rear_wheels():
left_motor_wheel = create_wheel(30, 15, (40, 30, 0)) right_motor_wheel = create_wheel(30, 15, (40, -30, 0)) return left_motor_wheel, right_motor_wheel
def create_front_wheel():
front_wheel = create_wheel(20, 10, (0, 0, 0)) return front_wheel
Assemble kart:
def create_kart():
kart_body = create_kart_body(150, 80, 30)
kart_frame = create_kart_frame(150, 80, 20)
arduino = create_arduino(60, 25, 10)
ultrasonic_1 = create_ultrasonic_sensor(10, 10, 5, (55, 20, 15)) ultrasonic_2 = create_ultrasonic_sensor(10, 10, 5, (55, -20, 15))
motor_controller = create_motor_controller(40, 20, 10, (100, 40, 0))
left_motor_wheel, right_motor_wheel = create_rear_wheels()
front_wheel = create_front_wheel()
return [kart_body, kart_frame, arduino, ultrasonic_1, ultrasonic_2, motor_controller,
left_motor_wheel, right_motor_wheel, front_wheel]
def main():
doc = FreeCAD.newDocument()
parts = create_kart()
for part in parts:
Part.show(part)
doc.recompute()
main()
Design board:
ISO-10303-21;
HEADER;
/* Generated by software containing ST-Developer
* from STEP Tools, Inc. (www.steptools.com)
*/
/* OPTION: using custom renumber hook */
FILE_DESCRIPTION(
/* description */ ('STEP AP242',
'CAx-IF Rec.Pracs.---Representation and Presentation of Product Manufa
cturing Information (PMI)---4.0---2014-10-13',
'CAx-IF Rec.Pracs.---3D Tessellated Geometry---0.4---2014-09-14','2;1'),
/* implementation_level */ '2;1');
FILE_NAME(
/* name */ '672886e6614d8b523e010712',
/* time_stamp */ '2024-11-04T08:33:42Z',
/* author */ (''),
/* organization */ (''),
/* preprocessor_version */ 'ST-DEVELOPER v20',
/* originating_system */ 'ONSHAPE BY PTC INC, 1.189',
/* authorisation */ ' ');
FILE_SCHEMA (('AP242_MANAGED_MODEL_BASED_3D_ENGINEERING_MIM_LF { 1 0 10303 442 1 1 4 }'));
ENDSEC;
DATA;
#10=SHAPE_REPRESENTATION_RELATIONSHIP('','',#283,#11);
#11=ADVANCED_BREP_SHAPE_REPRESENTATION('',(#281),#441);
#12=ORIENTED_EDGE('',*,*,#84,.F.);
#13=ORIENTED_EDGE('',*,*,#85,.F.);
#14=ORIENTED_EDGE('',*,*,#86,.T.);
#15=ORIENTED_EDGE('',*,*,#87,.T.);
#16=ORIENTED_EDGE('',*,*,#88,.T.);
#17=ORIENTED_EDGE('',*,*,#89,.F.);
#18=ORIENTED_EDGE('',*,*,#90,.F.);
#19=ORIENTED_EDGE('',*,*,#85,.T.);
#20=ORIENTED_EDGE('',*,*,#91,.T.);
#21=ORIENTED_EDGE('',*,*,#92,.F.);
#22=ORIENTED_EDGE('',*,*,#93,.F.);
#23=ORIENTED_EDGE('',*,*,#89,.T.);
#24=ORIENTED_EDGE('',*,*,#94,.T.);
#25=ORIENTED_EDGE('',*,*,#95,.F.);
#26=ORIENTED_EDGE('',*,*,#96,.F.);
#27=ORIENTED_EDGE('',*,*,#92,.T.);
#28=ORIENTED_EDGE('',*,*,#97,.T.);
#29=ORIENTED_EDGE('',*,*,#98,.F.);
#30=ORIENTED_EDGE('',*,*,#99,.F.);
#31=ORIENTED_EDGE('',*,*,#95,.T.);
#32=ORIENTED_EDGE('',*,*,#100,.T.);
#33=ORIENTED_EDGE('',*,*,#101,.F.);
#34=ORIENTED_EDGE('',*,*,#102,.F.);
#35=ORIENTED_EDGE('',*,*,#98,.T.);
#36=ORIENTED_EDGE('',*,*,#103,.T.);
#37=ORIENTED_EDGE('',*,*,#104,.F.);
#38=ORIENTED_EDGE('',*,*,#105,.F.);
#39=ORIENTED_EDGE('',*,*,#101,.T.);
#40=ORIENTED_EDGE('',*,*,#106,.T.);
#41=ORIENTED_EDGE('',*,*,#107,.F.);
#42=ORIENTED_EDGE('',*,*,#108,.F.);
#43=ORIENTED_EDGE('',*,*,#104,.T.);
#44=ORIENTED_EDGE('',*,*,#109,.T.);
#45=ORIENTED_EDGE('',*,*,#110,.F.);
#46=ORIENTED_EDGE('',*,*,#111,.F.);
#47=ORIENTED_EDGE('',*,*,#107,.T.);
#48=ORIENTED_EDGE('',*,*,#112,.T.);
#49=ORIENTED_EDGE('',*,*,#113,.F.);
#50=ORIENTED_EDGE('',*,*,#114,.F.);
#51=ORIENTED_EDGE('',*,*,#110,.T.);
#52=ORIENTED_EDGE('',*,*,#115,.T.);
#53=ORIENTED_EDGE('',*,*,#116,.F.);
#54=ORIENTED_EDGE('',*,*,#117,.F.);
#55=ORIENTED_EDGE('',*,*,#113,.T.);
#56=ORIENTED_EDGE('',*,*,#118,.F.);
#57=ORIENTED_EDGE('',*,*,#87,.F.);
#58=ORIENTED_EDGE('',*,*,#119,.T.);
#59=ORIENTED_EDGE('',*,*,#116,.T.);
#60=ORIENTED_EDGE('',*,*,#86,.F.);
#61=ORIENTED_EDGE('',*,*,#90,.T.);
#62=ORIENTED_EDGE('',*,*,#93,.T.);
#63=ORIENTED_EDGE('',*,*,#96,.T.);
#64=ORIENTED_EDGE('',*,*,#99,.T.);
#65=ORIENTED_EDGE('',*,*,#102,.T.);
#66=ORIENTED_EDGE('',*,*,#105,.T.);
#67=ORIENTED_EDGE('',*,*,#108,.T.);
#68=ORIENTED_EDGE('',*,*,#111,.T.);
#69=ORIENTED_EDGE('',*,*,#114,.T.);
#70=ORIENTED_EDGE('',*,*,#117,.T.);
#71=ORIENTED_EDGE('',*,*,#119,.F.);
#72=ORIENTED_EDGE('',*,*,#84,.T.);
#73=ORIENTED_EDGE('',*,*,#118,.T.);
#74=ORIENTED_EDGE('',*,*,#115,.F.);
#75=ORIENTED_EDGE('',*,*,#112,.F.);
#76=ORIENTED_EDGE('',*,*,#109,.F.);
#77=ORIENTED_EDGE('',*,*,#106,.F.);
#78=ORIENTED_EDGE('',*,*,#103,.F.);
#79=ORIENTED_EDGE('',*,*,#100,.F.);
#80=ORIENTED_EDGE('',*,*,#97,.F.);
#81=ORIENTED_EDGE('',*,*,#94,.F.);
#82=ORIENTED_EDGE('',*,*,#91,.F.);
#83=ORIENTED_EDGE('',*,*,#88,.F.);
#84=EDGE_CURVE('',#120,#121,#144,.T.);
#85=EDGE_CURVE('',#122,#120,#145,.T.);
#86=EDGE_CURVE('',#122,#123,#146,.T.);
#87=EDGE_CURVE('',#123,#121,#147,.T.);
#88=EDGE_CURVE('',#120,#124,#148,.T.);
#89=EDGE_CURVE('',#125,#124,#149,.T.);
#90=EDGE_CURVE('',#122,#125,#150,.T.);
#91=EDGE_CURVE('',#124,#126,#151,.T.);
#92=EDGE_CURVE('',#127,#126,#152,.T.);
#93=EDGE_CURVE('',#125,#127,#153,.T.);
#94=EDGE_CURVE('',#126,#128,#154,.T.);
#95=EDGE_CURVE('',#129,#128,#155,.T.);
#96=EDGE_CURVE('',#127,#129,#156,.T.);
#97=EDGE_CURVE('',#128,#130,#157,.T.);
#98=EDGE_CURVE('',#131,#130,#158,.T.);
#99=EDGE_CURVE('',#129,#131,#159,.T.);
#100=EDGE_CURVE('',#130,#132,#160,.T.);
#101=EDGE_CURVE('',#133,#132,#161,.T.);
#102=EDGE_CURVE('',#131,#133,#162,.T.);
#103=EDGE_CURVE('',#132,#134,#163,.T.);
#104=EDGE_CURVE('',#135,#134,#164,.T.);
#105=EDGE_CURVE('',#133,#135,#165,.T.);
#106=EDGE_CURVE('',#134,#136,#166,.T.);
#107=EDGE_CURVE('',#137,#136,#167,.T.);
#108=EDGE_CURVE('',#135,#137,#168,.T.);
#109=EDGE_CURVE('',#136,#138,#169,.T.);
#110=EDGE_CURVE('',#139,#138,#170,.T.);
#111=EDGE_CURVE('',#137,#139,#171,.T.);
#112=EDGE_CURVE('',#138,#140,#172,.T.);
#113=EDGE_CURVE('',#141,#140,#173,.T.);
#114=EDGE_CURVE('',#139,#141,#174,.T.);
#115=EDGE_CURVE('',#140,#142,#175,.T.);
#116=EDGE_CURVE('',#143,#142,#176,.T.);
#117=EDGE_CURVE('',#141,#143,#177,.T.);
#118=EDGE_CURVE('',#121,#142,#178,.T.);
#119=EDGE_CURVE('',#123,#143,#179,.T.);
#120=VERTEX_POINT('',#368);
#121=VERTEX_POINT('',#369);
#122=VERTEX_POINT('',#371);
#123=VERTEX_POINT('',#373);
#124=VERTEX_POINT('',#377);
#125=VERTEX_POINT('',#379);
#126=VERTEX_POINT('',#383);
#127=VERTEX_POINT('',#385);
#128=VERTEX_POINT('',#389);
#129=VERTEX_POINT('',#391);
#130=VERTEX_POINT('',#395);
#131=VERTEX_POINT('',#397);
#132=VERTEX_POINT('',#401);
#133=VERTEX_POINT('',#403);
#134=VERTEX_POINT('',#407);
#135=VERTEX_POINT('',#409);
#136=VERTEX_POINT('',#413);
#137=VERTEX_POINT('',#415);
#138=VERTEX_POINT('',#419);
#139=VERTEX_POINT('',#421);
#140=VERTEX_POINT('',#425);
#141=VERTEX_POINT('',#427);
#142=VERTEX_POINT('',#431);
#143=VERTEX_POINT('',#433);
#144=LINE('',#367,#180);
#145=LINE('',#370,#181);
#146=LINE('',#372,#182);
#147=LINE('',#374,#183);
#148=LINE('',#376,#184);
#149=LINE('',#378,#185);
#150=LINE('',#380,#186);
#151=LINE('',#382,#187);
#152=LINE('',#384,#188);
#153=LINE('',#386,#189);
#154=LINE('',#388,#190);
#155=LINE('',#390,#191);
#156=LINE('',#392,#192);
#157=LINE('',#394,#193);
#158=LINE('',#396,#194);
#159=LINE('',#398,#195);
#160=LINE('',#400,#196);
#161=LINE('',#402,#197);
#162=LINE('',#404,#198);
#163=LINE('',#406,#199);
#164=LINE('',#408,#200);
#165=LINE('',#410,#201);
#166=LINE('',#412,#202);
#167=LINE('',#414,#203);
#168=LINE('',#416,#204);
#169=LINE('',#418,#205);
#170=LINE('',#420,#206);
#171=LINE('',#422,#207);
#172=LINE('',#424,#208);
#173=LINE('',#426,#209);
#174=LINE('',#428,#210);
#175=LINE('',#430,#211);
#176=LINE('',#432,#212);
#177=LINE('',#434,#213);
#178=LINE('',#436,#214);
#179=LINE('',#437,#215);
#180=VECTOR('',#303,1.);
#181=VECTOR('',#304,1.);
#182=VECTOR('',#305,1.);
#183=VECTOR('',#306,1.);
#184=VECTOR('',#309,1.);
#185=VECTOR('',#310,1.);
#186=VECTOR('',#311,1.);
#187=VECTOR('',#314,1.);
#188=VECTOR('',#315,1.);
#189=VECTOR('',#316,1.);
#190=VECTOR('',#319,1.);
#191=VECTOR('',#320,1.);
#192=VECTOR('',#321,1.);
#193=VECTOR('',#324,1.);
#194=VECTOR('',#325,1.);
#195=VECTOR('',#326,1.);
#196=VECTOR('',#329,1.);
#197=VECTOR('',#330,1.);
#198=VECTOR('',#331,1.);
#199=VECTOR('',#334,1.);
#200=VECTOR('',#335,1.);
#201=VECTOR('',#336,1.);
#202=VECTOR('',#339,1.);
#203=VECTOR('',#340,1.);
#204=VECTOR('',#341,1.);
#205=VECTOR('',#344,1.);
#206=VECTOR('',#345,1.);
#207=VECTOR('',#346,1.);
#208=VECTOR('',#349,1.);
#209=VECTOR('',#350,1.);
#210=VECTOR('',#351,1.);
#211=VECTOR('',#354,1.);
#212=VECTOR('',#355,1.);
#213=VECTOR('',#356,1.);
#214=VECTOR('',#359,1.);
#215=VECTOR('',#360,1.);
#216=EDGE_LOOP('',(#12,#13,#14,#15));
#217=EDGE_LOOP('',(#16,#17,#18,#19));
#218=EDGE_LOOP('',(#20,#21,#22,#23));
#219=EDGE_LOOP('',(#24,#25,#26,#27));
#220=EDGE_LOOP('',(#28,#29,#30,#31));
#221=EDGE_LOOP('',(#32,#33,#34,#35));
#222=EDGE_LOOP('',(#36,#37,#38,#39));
#223=EDGE_LOOP('',(#40,#41,#42,#43));
#224=EDGE_LOOP('',(#44,#45,#46,#47));
#225=EDGE_LOOP('',(#48,#49,#50,#51));
#226=EDGE_LOOP('',(#52,#53,#54,#55));
#227=EDGE_LOOP('',(#56,#57,#58,#59));
#228=EDGE_LOOP('',(#60,#61,#62,#63,#64,#65,#66,#67,#68,#69,#70,#71));
#229=EDGE_LOOP('',(#72,#73,#74,#75,#76,#77,#78,#79,#80,#81,#82,#83));
#230=FACE_BOUND('',#216,.T.);
#231=FACE_BOUND('',#217,.T.);
#232=FACE_BOUND('',#218,.T.);
#233=FACE_BOUND('',#219,.T.);
#234=FACE_BOUND('',#220,.T.);
#235=FACE_BOUND('',#221,.T.);
#236=FACE_BOUND('',#222,.T.);
#237=FACE_BOUND('',#223,.T.);
#238=FACE_BOUND('',#224,.T.);
#239=FACE_BOUND('',#225,.T.);
#240=FACE_BOUND('',#226,.T.);
#241=FACE_BOUND('',#227,.T.);
#242=FACE_BOUND('',#228,.T.);
#243=FACE_BOUND('',#229,.T.);
#244=PLANE('',#285);
#245=PLANE('',#286);
#246=PLANE('',#287);
#247=PLANE('',#288);
#248=PLANE('',#289);
#249=PLANE('',#290);
#250=PLANE('',#291);
#251=PLANE('',#292);
#252=PLANE('',#293);
#253=PLANE('',#294);
#254=PLANE('',#295);
#255=PLANE('',#296);
#256=PLANE('',#297);
#257=PLANE('',#298);
#258=ADVANCED_FACE('',(#230),#244,.T.);
#259=ADVANCED_FACE('',(#231),#245,.F.);
#260=ADVANCED_FACE('',(#232),#246,.F.);
#261=ADVANCED_FACE('',(#233),#247,.F.);
#262=ADVANCED_FACE('',(#234),#248,.F.);
#263=ADVANCED_FACE('',(#235),#249,.F.);
#264=ADVANCED_FACE('',(#236),#250,.F.);
#265=ADVANCED_FACE('',(#237),#251,.F.);
#266=ADVANCED_FACE('',(#238),#252,.F.);
#267=ADVANCED_FACE('',(#239),#253,.F.);
#268=ADVANCED_FACE('',(#240),#254,.F.);
#269=ADVANCED_FACE('',(#241),#255,.T.);
#270=ADVANCED_FACE('',(#242),#256,.T.);
#271=ADVANCED_FACE('',(#243),#257,.F.);
#272=CLOSED_SHELL('',(#258,#259,#260,#261,#262,#263,#264,#265,#266,#267,
#268,#269,#270,#271));
#273=STYLED_ITEM('',(#274),#281);
#274=PRESENTATION_STYLE_ASSIGNMENT((#275));
#275=SURFACE_STYLE_USAGE(.BOTH.,#276);
#276=SURFACE_SIDE_STYLE('',(#277));
#277=SURFACE_STYLE_FILL_AREA(#278);
#278=FILL_AREA_STYLE('',(#279));
#279=FILL_AREA_STYLE_COLOUR('',#280);
#280=COLOUR_RGB('',0.615686274509804,0.811764705882353,0.929411764705882);
#281=MANIFOLD_SOLID_BREP('Part 1',#272);
#282=SHAPE_DEFINITION_REPRESENTATION(#446,#283);
#283=SHAPE_REPRESENTATION('Part 1',(#284),#441);
#284=AXIS2_PLACEMENT_3D('',#365,#299,#300);
#285=AXIS2_PLACEMENT_3D('',#366,#301,#302);
#286=AXIS2_PLACEMENT_3D('',#375,#307,#308);
#287=AXIS2_PLACEMENT_3D('',#381,#312,#313);
#288=AXIS2_PLACEMENT_3D('',#387,#317,#318);
#289=AXIS2_PLACEMENT_3D('',#393,#322,#323);
#290=AXIS2_PLACEMENT_3D('',#399,#327,#328);
#291=AXIS2_PLACEMENT_3D('',#405,#332,#333);
#292=AXIS2_PLACEMENT_3D('',#411,#337,#338);
#293=AXIS2_PLACEMENT_3D('',#417,#342,#343);
#294=AXIS2_PLACEMENT_3D('',#423,#347,#348);
#295=AXIS2_PLACEMENT_3D('',#429,#352,#353);
#296=AXIS2_PLACEMENT_3D('',#435,#357,#358);
#297=AXIS2_PLACEMENT_3D('',#438,#361,#362);
#298=AXIS2_PLACEMENT_3D('',#439,#363,#364);
#299=DIRECTION('',(0.,0.,1.));
#300=DIRECTION('',(1.,0.,0.));
#301=DIRECTION('',(-1.,0.,0.));
#302=DIRECTION('',(0.,1.,0.));
#303=DIRECTION('',(0.,0.,-1.));
#304=DIRECTION('',(0.,-1.,0.));
#305=DIRECTION('',(0.,0.,-1.));
#306=DIRECTION('',(0.,-1.,0.));
#307=DIRECTION('',(0.,0.,-1.));
#308=DIRECTION('',(0.,1.,0.));
#309=DIRECTION('',(1.,0.,0.));
#310=DIRECTION('',(0.,-1.,0.));
#311=DIRECTION('',(1.,0.,0.));
#312=DIRECTION('',(1.,0.,0.));
#313=DIRECTION('',(0.,-1.,0.));
#314=DIRECTION('',(0.,0.,1.));
#315=DIRECTION('',(0.,-1.,0.));
#316=DIRECTION('',(0.,0.,1.));
#317=DIRECTION('',(0.,0.,-1.));
#318=DIRECTION('',(0.,1.,0.));
#319=DIRECTION('',(1.,0.,0.));
#320=DIRECTION('',(0.,-1.,0.));
#321=DIRECTION('',(1.,0.,0.));
#322=DIRECTION('',(1.,0.,0.));
#323=DIRECTION('',(0.,-1.,0.));
#324=DIRECTION('',(0.,0.,1.));
#325=DIRECTION('',(0.,-1.,0.));
#326=DIRECTION('',(0.,0.,1.));
#327=DIRECTION('',(0.,0.,-1.));
#328=DIRECTION('',(0.,1.,0.));
#329=DIRECTION('',(1.,0.,0.));
#330=DIRECTION('',(0.,-1.,0.));
#331=DIRECTION('',(1.,0.,0.));
#332=DIRECTION('',(-1.,0.,0.));
#333=DIRECTION('',(0.,1.,0.));
#334=DIRECTION('',(0.,0.,-1.));
#335=DIRECTION('',(0.,-1.,0.));
#336=DIRECTION('',(0.,0.,-1.));
#337=DIRECTION('',(0.,0.,-1.));
#338=DIRECTION('',(0.,1.,0.));
#339=DIRECTION('',(1.,0.,0.));
#340=DIRECTION('',(0.,-1.,0.));
#341=DIRECTION('',(1.,0.,0.));
#342=DIRECTION('',(-1.,0.,0.));
#343=DIRECTION('',(0.,1.,0.));
#344=DIRECTION('',(0.,0.,-1.));
#345=DIRECTION('',(0.,-1.,0.));
#346=DIRECTION('',(0.,0.,-1.));
#347=DIRECTION('',(0.,0.,-1.));
#348=DIRECTION('',(0.,1.,0.));
#349=DIRECTION('',(1.,0.,0.));
#350=DIRECTION('',(0.,-1.,0.));
#351=DIRECTION('',(1.,0.,0.));
#352=DIRECTION('',(-1.,0.,0.));
#353=DIRECTION('',(0.,1.,0.));
#354=DIRECTION('',(0.,0.,-1.));
#355=DIRECTION('',(0.,-1.,0.));
#356=DIRECTION('',(0.,0.,-1.));
#357=DIRECTION('',(0.,0.,-1.));
#358=DIRECTION('',(0.,1.,0.));
#359=DIRECTION('',(1.,0.,0.));
#360=DIRECTION('',(1.,0.,0.));
#361=DIRECTION('',(0.,1.,0.));
#362=DIRECTION('',(1.,0.,0.));
#363=DIRECTION('',(0.,1.,0.));
#364=DIRECTION('',(1.,0.,0.));
#365=CARTESIAN_POINT('',(0.,0.,0.));
#366=CARTESIAN_POINT('',(0.,0.005,-0.025));
#367=CARTESIAN_POINT('',(0.,0.,-0.025));
#368=CARTESIAN_POINT('',(0.,0.,0.));
#369=CARTESIAN_POINT('',(-6.93889390390723E-18,0.,-0.05));
#370=CARTESIAN_POINT('',(0.,0.005,0.));
#371=CARTESIAN_POINT('',(0.,0.005,0.));
#372=CARTESIAN_POINT('',(0.,0.005,-0.025));
#373=CARTESIAN_POINT('',(-6.93889390390723E-18,0.005,-0.05));
#374=CARTESIAN_POINT('',(-6.93889390390723E-18,0.005,-0.05));
#375=CARTESIAN_POINT('',(0.015,0.005,0.));
#376=CARTESIAN_POINT('',(0.015,0.,0.));
#377=CARTESIAN_POINT('',(0.03,0.,0.));
#378=CARTESIAN_POINT('',(0.03,0.005,0.));
#379=CARTESIAN_POINT('',(0.03,0.005,0.));
#380=CARTESIAN_POINT('',(0.015,0.005,0.));
#381=CARTESIAN_POINT('',(0.03,0.005,0.035));
#382=CARTESIAN_POINT('',(0.03,0.,0.035));
#383=CARTESIAN_POINT('',(0.03,0.,0.07));
#384=CARTESIAN_POINT('',(0.03,0.005,0.07));
#385=CARTESIAN_POINT('',(0.03,0.005,0.07));
#386=CARTESIAN_POINT('',(0.03,0.005,0.035));
#387=CARTESIAN_POINT('',(0.045,0.005,0.07));
#388=CARTESIAN_POINT('',(0.045,0.,0.07));
#389=CARTESIAN_POINT('',(0.06,0.,0.07));
#390=CARTESIAN_POINT('',(0.06,0.005,0.07));
#391=CARTESIAN_POINT('',(0.06,0.005,0.07));
#392=CARTESIAN_POINT('',(0.045,0.005,0.07));
#393=CARTESIAN_POINT('',(0.06,0.005,0.12));
#394=CARTESIAN_POINT('',(0.06,0.,0.12));
#395=CARTESIAN_POINT('',(0.06,0.,0.17));
#396=CARTESIAN_POINT('',(0.06,0.005,0.17));
#397=CARTESIAN_POINT('',(0.06,0.005,0.17));
#398=CARTESIAN_POINT('',(0.06,0.005,0.12));
#399=CARTESIAN_POINT('',(0.075,0.005,0.17));
#400=CARTESIAN_POINT('',(0.075,0.,0.17));
#401=CARTESIAN_POINT('',(0.09,0.,0.17));
#402=CARTESIAN_POINT('',(0.09,0.005,0.17));
#403=CARTESIAN_POINT('',(0.09,0.005,0.17));
#404=CARTESIAN_POINT('',(0.075,0.005,0.17));
#405=CARTESIAN_POINT('',(0.09,0.005,0.12));
#406=CARTESIAN_POINT('',(0.09,0.,0.12));
#407=CARTESIAN_POINT('',(0.09,0.,0.07));
#408=CARTESIAN_POINT('',(0.09,0.005,0.07));
#409=CARTESIAN_POINT('',(0.09,0.005,0.07));
#410=CARTESIAN_POINT('',(0.09,0.005,0.12));
#411=CARTESIAN_POINT('',(0.105,0.005,0.07));
#412=CARTESIAN_POINT('',(0.105,0.,0.07));
#413=CARTESIAN_POINT('',(0.12,0.,0.07));
#414=CARTESIAN_POINT('',(0.12,0.005,0.07));
#415=CARTESIAN_POINT('',(0.12,0.005,0.07));
#416=CARTESIAN_POINT('',(0.105,0.005,0.07));
#417=CARTESIAN_POINT('',(0.12,0.005,0.035));
#418=CARTESIAN_POINT('',(0.12,0.,0.035));
#419=CARTESIAN_POINT('',(0.12,0.,0.));
#420=CARTESIAN_POINT('',(0.12,0.005,0.));
#421=CARTESIAN_POINT('',(0.12,0.005,0.));
#422=CARTESIAN_POINT('',(0.12,0.005,0.035));
#423=CARTESIAN_POINT('',(0.135,0.005,0.));
#424=CARTESIAN_POINT('',(0.135,0.,0.));
#425=CARTESIAN_POINT('',(0.15,0.,0.));
#426=CARTESIAN_POINT('',(0.15,0.005,0.));
#427=CARTESIAN_POINT('',(0.15,0.005,0.));
#428=CARTESIAN_POINT('',(0.135,0.005,0.));
#429=CARTESIAN_POINT('',(0.15,0.005,-0.025));
#430=CARTESIAN_POINT('',(0.15,0.,-0.025));
#431=CARTESIAN_POINT('',(0.15,0.,-0.05));
#432=CARTESIAN_POINT('',(0.15,0.005,-0.05));
#433=CARTESIAN_POINT('',(0.15,0.005,-0.05));
#434=CARTESIAN_POINT('',(0.15,0.005,-0.025));
#435=CARTESIAN_POINT('',(0.075,0.005,-0.05));
#436=CARTESIAN_POINT('',(0.075,0.,-0.05));
#437=CARTESIAN_POINT('',(0.075,0.005,-0.05));
#438=CARTESIAN_POINT('',(0.075,0.005,0.06));
#439=CARTESIAN_POINT('',(0.075,0.,0.06));
#440=MECHANICAL_DESIGN_GEOMETRIC_PRESENTATION_REPRESENTATION('',(#273),
#441);
#441=(
GEOMETRIC_REPRESENTATION_CONTEXT(3)
GLOBAL_UNCERTAINTY_ASSIGNED_CONTEXT((#442))
GLOBAL_UNIT_ASSIGNED_CONTEXT((#445,#444,#443))
REPRESENTATION_CONTEXT('Part 1','TOP_LEVEL_ASSEMBLY_PART')
);
#442=UNCERTAINTY_MEASURE_WITH_UNIT(LENGTH_MEASURE(5.E-6),#445,
'DISTANCE_ACCURACY_VALUE','Maximum Tolerance applied to model');
#443=(
NAMED_UNIT(*)
SI_UNIT($,.STERADIAN.)
SOLID_ANGLE_UNIT()
);
#444=(
NAMED_UNIT(*)
PLANE_ANGLE_UNIT()
SI_UNIT($,.RADIAN.)
);
#445=(
LENGTH_UNIT()
NAMED_UNIT(*)
SI_UNIT($,.METRE.)
);
#446=PRODUCT_DEFINITION_SHAPE('','',#447);
#447=PRODUCT_DEFINITION('','',#449,#448);
#448=PRODUCT_DEFINITION_CONTEXT('',#455,'design');
#449=PRODUCT_DEFINITION_FORMATION_WITH_SPECIFIED_SOURCE('','',#451,
.NOT_KNOWN.);
#450=PRODUCT_RELATED_PRODUCT_CATEGORY('','',(#451));
#451=PRODUCT('Part 1','Part 1','Part 1',(#453));
#452=PRODUCT_CATEGORY('','');
#453=PRODUCT_CONTEXT('',#455,'mechanical');
#454=APPLICATION_PROTOCOL_DEFINITION('international standard',
'ap242_managed_model_based_3d_engineering',2011,#455);
#455=APPLICATION_CONTEXT('managed model based 3d engineering');
ENDSEC;
END-ISO-10303-21;
Design and assemble netlist:
(kicad_netlist (version 4)
(components
(comp (value Arduino) (footprint Arduino:Uno) (ref U1)
(pin 1 (node 1)) (pin 2 (node 2)) (pin 3 (node 3)) (pin 4 (node 4)) (pin 5 (node 5))
(pin 6 (node 6)) (pin 7 (node 7)) (pin 8 (node 8)) (pin 9 (node 9)) (pin 10 (node 10))
(pin 11 (node 11)) (pin 12 (node 12)) (pin 13 (node 13)) (pin 14 (node 14)) (pin 15 (node 15))
(pin 16 (node 16)) (pin 17 (node 17)) (pin 18 (node 18)) (pin 19 (node 19)) (pin 20 (node 20))
)
(comp (value HC-SR04_1) (footprint Sensor:HC-SR04) (ref U2)
(pin 1 (node 21)) (pin 2 (node 22)) (pin 3 (node 23)) (pin 4 (node 24))
)
(comp (value HC-SR04_2) (footprint Sensor:HC-SR04) (ref U3)
(pin 1 (node 25)) (pin 2 (node 26)) (pin 3 (node 27)) (pin 4 (node 28))
)
(comp (value L293D) (footprint IC:SOIC-16) (ref U4)
(pin 1 (node 29)) (pin 2 (node 30)) (pin 3 (node 31)) (pin 4 (node 32))
(pin 5 (node 33)) (pin 6 (node 34)) (pin 7 (node 35)) (pin 8 (node 36))
(pin 9 (node 37)) (pin 10 (node 38)) (pin 11 (node 39)) (pin 12 (node 40))
(pin 13 (node 41)) (pin 14 (node 42)) (pin 15 (node 43)) (pin 16 (node 44))
)
(comp (value Motor_1) (footprint Motor:SMD) (ref M1)
(pin 1 (node 45)) (pin 2 (node 46))
)
(comp (value Motor_2) (footprint Motor:SMD) (ref M2)
(pin 1 (node 47)) (pin 2 (node 48))
)
(comp (value Battery_Box) (footprint Battery:Box) (ref B1)
(pin 1 (node 49)) (pin 2 (node 50))
)
)
(nets
(net (code 1) (node 1) (node 3) (node 4) (node 5)) ; Arduino power
(net (code 2) (node 21) (node 22)) ; HC-SR04 1 Echo and Trigger pins
(net (code 3) (node 23) (node 24)) ; HC-SR04 1 Trigger and Echo pins
(net (code 4) (node 25) (node 26)) ; HC-SR04 2 Echo and Trigger pins
(net (code 5) (node 27) (node 28)) ; HC-SR04 2 Trigger and Echo pins
(net (code 6) (node 29) (node 31)) ; L293D input and output pins
(net (code 7) (node 32) (node 33)) ; L293D control pins
(net (code 8) (node 34) (node 35)) ; L293D control pins
(net (code 9) (node 36) (node 37)) ; Motor power pins
(net (code 10) (node 39) (node 40)) ; Motor driver control
(net (code 11) (node 41) (node 42)) ; Motor power
(net (code 12) (node 43) (node 44)) ; Motor drive connection
(net (code 13) (node 45) (node 46)) ; Motor 1
(net (code 14) (node 47) (node 48)) ; Motor 2
(net (code 15) (node 49) (node 50)) ; Battery Box connection
)
)
Perform detection and drive control:
import unittest
import FreeCAD
import Part
from FreeCAD import Base
def create_wheel(radius, width, position):
wheel = Part.makeCylinder(radius, width)
wheel.translate(Base.Vector(position[0], position[1], position[2]))
return wheel
def create_kart_body(length, width, height):
body = Part.makeBox(length, width, height)
return body
def create_kart_frame(length, width, height):
frame = Part.makeBox(length, width, height)
frame.translate(Base.Vector(0, 0, height / 2)) return frame
def create_front_wheel():
front_wheel = create_wheel(20, 10, (0, 0, 0)) return front_wheel
def create_kart():
kart_body = create_kart_body(150, 80, 30)
kart_frame = create_kart_frame(150, 80, 20)
front_wheel = create_front_wheel()
return [kart_body, kart_frame, front_wheel]
class TestKartModel(unittest.TestCase):
def setUp(self):
"""Set up the environment for each test"""
self.parts = create_kart() self.kart_body = self.parts[0]
self.kart_frame = self.parts[1]
self.front_wheel = self.parts[2]
def test_kart_body_dimensions(self):
"""Test the dimensions of the kart body"""
length, width, height = 150, 80, 30
self.assertEqual(self.kart_body.Length, length)
self.assertEqual(self.kart_body.Width, width)
self.assertEqual(self.kart_body.Height, height)
def test_kart_frame_position(self):
"""Test the position of the kart frame"""
expected_position = Base.Vector(0, 0, 10) frame_position = self.kart_frame.CenterOfMass
self.assertEqual(frame_position, expected_position)
def test_front_wheel_position(self):
"""Test the position of the front wheel"""
expected_position = Base.Vector(0, 0, 0) wheel_position = self.front_wheel.CenterOfMass
self.assertEqual(wheel_position, expected_position)
def test_wheel_dimensions(self):
"""Test the dimensions of the front wheel"""
radius, width = 20, 10
wheel_radius = self.front_wheel.Radius
wheel_width = self.front_wheel.Height
self.assertEqual(wheel_radius, radius)
self.assertEqual(wheel_width, width)
def test_assembly(self):
"""Test if all components are assembled correctly"""
self.assertIsInstance(self.kart_body, Part.Feature)
self.assertIsInstance(self.kart_frame, Part.Feature)
self.assertIsInstance(self.front_wheel, Part.Feature)
self.assertEqual(len(self.parts), 3)
if __name__ == '__main__':
unittest.main()
Writing programs:
#ifndef PID_HPP
#define PID_HPP
#ifdef ARDUINO
#include
#include
#define to_string(a) String(a)
#else
#include
#include
#define String string
using namespace std;
#endif
#define MAX(a, b) (((a) > (b)) ? (a) : (b))
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
#define ABS(a) (((a) < 0) ? -(a) : (a))
#define COPYSIGN(a, b) (((b) < 0) ? -ABS(a) : ABS(a))
struct Data {
unsigned long dt;
double e;
};
struct Node {
Data data;
Node* prev;
};
struct List {
Node* head;
Node* tail;
unsigned int size;
};
class PID {
public:
const double maxIntTm, maxAmp, minKp, maxKp, rTiM, TdM, TddM, eDPm, eDPa;
const unsigned long session;
double Kp, Ki, Kd, Kdd, preE, preDv, intg, preOut;
unsigned long preT;
List dtXs;
PID(double maxIntTm, double maxAmp, double minKp, double maxKp, double rTiM, double TdM, double TddM, double eDPm, double eDPa, unsigned long session, double Kp, double preE=0, unsigned long preT=0)
: maxIntTm(maxIntTm), maxAmp(maxAmp), minKp(minKp), maxKp(maxKp), rTiM(rTiM), TdM(TdM), TddM(TddM), eDPm(eDPm), eDPa(eDPa), session(session), Kp(Kp), Ki(0), Kd(0), Kdd(0), preE(preE), preDv(0), intg(0), preOut(0), preT(preT), dtXs({nullptr, nullptr, 0}) {}
double update(double e, unsigned long timestamp, String* debug = nullptr) {
unsigned long dt = timestamp - preT;
preT = timestamp;
if (dt == 0) {
if (debug != nullptr) {
*debug += "dt=0\n";
}
return preOut;
}
double amp, ap;
if (session>0) {
unsigned long S = 0;
amp = 0;
if (dtXs.size > 1) {
Node* pos = dtXs.tail;
while (dtXs.size > 1 && pos != nullptr) {
S += pos->data.dt;
amp = MAX(ABS(pos->data.e), amp);
if (S > session) {
dtXs.head = pos;
Node* cur = pos->prev;
pos->prev = nullptr;
while (cur != nullptr) {
Node* tmp = cur->prev;
delete cur;
cur = tmp;
dtXs.size--;
}
break;
} else pos = pos->prev;
}
} else if (dtXs.size == 1) {
amp = ABS(dtXs.tail->data.e);
}
ap = exp(-amp / maxAmp);
} else {
amp=0;
ap=1;
}
Ki = Kp * rTiM * ap;
Kd = Kp * TdM * ap;
Kdd = Kd * TddM;
double dv = (e - preE) / dt;
double dd = (dv - preDv) / dt;
intg += e * dt;
double intTm = Ki * intg;
if (ABS(intTm) > maxIntTm) {
intg = COPYSIGN(maxIntTm / Ki, intTm);
intTm = COPYSIGN(maxIntTm, intTm);
}
double propTm = Kp * e;
double dvTm = Kd * dv;
double ddTm = Kdd * dd;
double out = propTm + intTm + dvTm + ddTm;
double eDP = e * dv;
if (ABS(eDP) > eDPm) {
Kp *= (eDP < 0) ? (1+eDPa) : (1-eDPa);
Kp = MIN(MAX(Kp, minKp), maxKp);
}
if (debug != nullptr) {
*debug += "update() called\ndt: " + to_string(dt) + ", e: " + to_string(e) + ", dv: " + to_string(dv) + ", dd: " + to_string(dd) +", amp: " + to_string(amp) +", ap: " + to_string(ap) + ",\nKp: " + to_string(Kp) + ", Ki: " + to_string(Ki) + ", Kd: " + to_string(Kd) + ", Kdd: " + to_string(Kdd) + ",\npropTm: " + to_string(propTm) + ", intTm: " + to_string(intTm) + ", dvTm: " + to_string(dvTm) + ", ddTm: " + to_string(ddTm) + ",\neDP: " + to_string(eDP) + ", out: " + to_string(out) + "\n";
}
preE = e;
preDv = dv;
preOut = out;
if (session>0) {
Node* nn = new Node{{dt, e}, dtXs.tail};
dtXs.tail = nn;
if (dtXs.size == 0) dtXs.head = nn;
dtXs.size++;
}
return out;
}
};
#endif
Perform simulation:
#ifndef TESTCAR_HPP
#define TESTCAR_HPP
#include
#include
#include
#include
#include
#include
#include
#include "PID.hpp"
using namespace std;
class Car
{
public:
Car(double initialSpeed, double initialPosition)
: speed(initialSpeed), position(initialPosition) {}
void updatePosition(double timeInterval)
{
position += speed * timeInterval;
}
void changeSpeed(double newSpeed)
{
speed = newSpeed;
}
double getPosition() const
{
return position;
}
double getSpeed() const
{
return speed;
}
double speed;
double position;
};
void pidDebug(string message)
{
cout << message;
}
double sum_last_squared(const std::vector &v, double prop)
{
if (v.empty())
return 0.0;
size_t n = v.size();
size_t count = static_cast(std::ceil(n * prop));
size_t start_index = n - count;
double sum_of_squares = std::accumulate(v.begin() + start_index, v.end(), 0.0,
[](double sum, double value) {
return sum + value * value;
});
return sum_of_squares;
}
vector velocities(int steps)
{
vector velocities(steps, 0.0);
double acceleration = 1;
double error = 0.2;
int phase_length = steps / 6;
double velocity = 0.0;
for (int i = 0; i < phase_length; ++i)
{
velocity += acceleration + ((i % 3 == 0) ? error : ((i % 3 == 1) ? (-error) : 0));
velocities[i] = velocity;
}
for (int i = phase_length; i < 2 * phase_length; ++i)
{
velocity += ((i % 3 == 0) ? error : ((i % 3 == 1) ? (-error) : 0));
velocities[i] = velocity;
}
for (int i = 2 * phase_length; i < 4 * phase_length; ++i)
{
velocity -= acceleration + ((i % 3 == 0) ? error : ((i % 3 == 1) ? (-error) : 0));
velocities[i] = velocity;
}
for (int i = 4 * phase_length; i < 5 * phase_length; ++i)
{
velocity += acceleration + ((i % 3 == 0) ? error : ((i % 3 == 1) ? (-error) : 0));
velocities[i] = velocity;
}
for (int i = 5 * phase_length; i < steps; ++i)
{
if (ABS(velocity) >= (error * (steps - i)))
{
velocity += COPYSIGN(velocity, error);
}
else
{
velocity += ((i % 3 == 0) ? error : ((i % 3 == 1) ? (-error) : 0));
}
velocities[i] = velocity;
}
return velocities;
}
vector test(double maxIntTm, double maxAmp, double minKp, double maxKp, double rTiM, double TdM, double TddM, double eDPm, double eDPa, unsigned long session, double Kp, double preE = 0, unsigned long preT = 0, double timeInterval = 10, int steps = 50, bool debug = false)
{
double leaderInitialSpeed = 0;
double followerInitialSpeed = 0;
double initialDistance = 0;
vector vec;
vector vol = velocities(steps);
Car leader(leaderInitialSpeed, initialDistance);
Car follower(followerInitialSpeed, 0);
PID pid = PID(maxIntTm, maxAmp, minKp, maxKp, rTiM, TdM, TddM, eDPm, eDPa, session, Kp, preE, preT);
cout << fixed << setprecision(4);
for (int i = 0; i < steps; i++)
{
leader.changeSpeed(leader.getSpeed() + vol[i]);
leader.updatePosition(timeInterval);
follower.updatePosition(timeInterval);
double distanceToLeader = leader.getPosition() - follower.getPosition();
vec.push_back(distanceToLeader);
string* ptr = nullptr;
if (debug) {
cout << "TimeStep: " << i << ", distanceToLeader: " << distanceToLeader << ", FollowerSpeed: " << follower.getSpeed() << ", LeaderSpeed: " << leader.getSpeed() << "\n";
ptr = new string;
}
double fS = pid.update(distanceToLeader, i * timeInterval, ptr);
follower.changeSpeed(follower.getSpeed() + MAX(-511, MIN(fS, 511)));
if (debug) {
cout << *ptr;
delete ptr;
}
}
return vec;
}
struct pidTest
{
double maxIntTm, maxAmp, minKp, maxKp, rTiM, TdM, TddM, eDPm, eDPa;
unsigned long session;
double Kp, result;
};
bool write_results(const vector &data, const string &filename)
{
auto sorted_data = data;
sort(sorted_data.begin(), sorted_data.end(), [](const pidTest &a, const pidTest &b) {
return a.result < b.result;
});
ofstream outfile(filename);
if (!outfile)
{
cerr << "Error opening file for writing." << endl;
return 0;
}
outfile << "maxIntTm,maxAmp,minKp,maxKp,rTiM,TdM,TddM,eDPm,eDPa,session,Kp,result\n";
for (size_t i = 0; i < sorted_data.size(); ++i)
{
const pidTest &pt = sorted_data[i];
outfile << pt.maxIntTm << "," << pt.maxAmp << "," << pt.minKp << "," << pt.maxKp << ","
<< pt.rTiM << "," << pt.TdM << "," << pt.TddM << "," << pt.eDPm << "," << pt.eDPa << ","
<< pt.session << "," << pt.Kp << "," << pt.result << "\n";
}
outfile.close();
cout << "Data written to " << filename << endl;
return 1;
}
struct ParameterRange
{
double start;
double end;
double step;
};
#endif
Optimize software performance:
#ifndef OPTIMIZATION_HPP
#define OPTIMIZATION_HPP
#include "testCar.hpp"
using namespace std;
int optimize(vector ranges)
{
vector results;
for (double maxIntTm = ranges[0].start; maxIntTm <= ranges[0].end; maxIntTm += ranges[0].step)
{
for (double maxAmp = ranges[1].start; maxAmp <= ranges[1].end; maxAmp += ranges[1].step)
{
for (double minKp = ranges[2].start; minKp <= ranges[2].end; minKp += ranges[2].step)
{
for (double maxKp = max(minKp, ranges[3].start);
maxKp <= ranges[3].end; maxKp += ranges[3].step)
{
for (double rTiM = ranges[4].start; rTiM <= ranges[4].end; rTiM += ranges[4].step)
{
for (double TdM = ranges[5].start; TdM <= ranges[5].end; TdM += ranges[5].step)
{
for (double TddM = ranges[6].start; TddM <= ranges[6].end; TddM += ranges[6].step)
{
for (double eDPm = ranges[7].start; eDPm <= ranges[7].end; eDPm += ranges[7].step)
{
for (double eDPa = ranges[8].start; eDPa <= ranges[8].end; eDPa += ranges[8].step)
{
for (unsigned long session = ranges[9].start; session <= ranges[9].end; session += ranges[9].step)
{
for (double Kp = MAX(minKp, ranges[10].start);
Kp <= MIN(maxKp, ranges[10].end); Kp += ranges[10].step)
{
vector tmp = test(maxIntTm, maxAmp, minKp, maxKp, rTiM, TdM, TddM, eDPm, eDPa, session, Kp);
double result = sum_last_squared(tmp, 0.5);
results.push_back(pidTest({maxIntTm, maxAmp, minKp, maxKp, rTiM, TdM, TddM, eDPm, eDPa, session, Kp, result}));
}
}
}
}
}
}
}
}
}
}
}
if (!write_results(results, "results.csv"))
{
cerr << "Error writing results to file." << endl;
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
#endif
#include "optimization.hpp" // or #include "optimization_multithread.hpp"
int main() {
// Note that steps can't be zero
vector ranges = {
{461.9, 461.9, 1}, // maxIntTm
{517.9, 517.9, 1}, // maxAmp
{0.00265, 0.00265, 1}, // minKp
{0.472, 0.472, 1}, // maxKp
{1, 1, 1}, // rTiM
{65.37, 65.37, 1}, // TdM
{1.7, 1.7, 1}, // TddM
{125, 125, 1}, // eDPm
{0.15, 0.15, 1}, // eDPa
{140, 140, 10}, // session
{0.2436, 0.2436, 1} // Kp
};
return optimize(ranges);
}
Perform hardware and software compatibility test:
#include "PID.hpp"
#define TARGET_DISTANCE 10
#define DISTANCE_BETWEEN_ULTRASONIC 5
#define DIFFERENTIAL_SPEED_MULTIPLIER 0.8
#define POSITIVE_SPEED_MULTIPLIER 1
#define NEGATIVE_SPEED_MULTIPLIER 1.2
PID avgPID(461.9, 517.9, 0.00265, 0.472, 1, 65.37, 1.7, 125, 0.15, 140, 0.2436);
PID difPID(461.9, 517.9, 0.00265, 0.472, 1, 65.37, 1.7, 125, 0.15, 140, 0.2436);
double avgV;
double difV;
inline double leftIn() {
return 0;
}
inline double rightIn() {
return 0;
}
void setup() {
avgV = 0;
difV = 0;
}
void loop() {
double left = leftIn();
double right = rightIn();
double avg = left + right;
double dif = atan2(left - right, DISTANCE_BETWEEN_ULTRASONIC);
avgV += avgPID.update((avg - TARGET_DISTANCE), millis());
difV += difPID.update((dif - TARGET_DISTANCE), millis()) * DIFFERENTIAL_SPEED_MULTIPLIER;
double leftV = avgV + difV;
leftV *= (leftV>0) ? POSITIVE_SPEED_MULTIPLIER : NEGATIVE_SPEED_MULTIPLIER;
double rightV = avgV - difV;
rightV *= (rightV>0) ? POSITIVE_SPEED_MULTIPLIER : NEGATIVE_SPEED_MULTIPLIER;
leftOut(leftV);
rightOut(rightV);
}
inline void leftOut(double output) {
Serial.println("Left: " + String(output));
}
inline void rightOut(double output) {
Serial.println("Right: " + String(output));
}
Inspiration
Beforehand:
The development of this PID (Proportional-Integral-Derivative) controller project is rooted in control theory, a field of engineering that focuses on the behavior of dynamic systems. What makes us take the initiative is the auto-following kart lesson in our living technology class. We aim to make an auto-following kart from scratch with Arduino and wooden boards and make it as quality as possible on a constrained budget.
Control Theory and PID Controllers
The development of this PID (Proportional-Integral-Derivative) controller project is rooted in control theory, a field of engineering that focuses on the behavior of dynamic systems. PID controllers are a cornerstone of modern control systems. They are designed to automatically adjust system outputs based on feedback to minimize error, allowing for precise control of dynamic systems. The PID controller operates based on three fundamental components:
-
Proportional Control (P): Responds proportionally to the current error value, providing immediate correction based on how far the system is from the desired state.
- Integral Control (I): Addresses accumulated past errors, ensuring that the system reaches the desired state even if there are persistent biases.
- Derivative Control (D): Predicts future errors based on the rate of change of the error, providing a damping effect to reduce overshoot and oscillation.
The tuning of these parameters is crucial for achieving optimal performance. The traditional Ziegler–Nichols tuning method has served as a foundational approach for many engineers, providing empirical guidelines for setting PID parameters based on system behavior.
Arduino
Arduino boards offer an accessible platform for students and hobbyists to experiment with hardware and software integration, facilitating hands-on learning experiences in technology classes.
In living technology classes, we engage with Arduino to build auto-following kart projects that require real-time control systems. This scenario highlights the importance of tuning PID controllers to achieve desired behaviors in robotics and automotive systems.
This PID controller serves as an imperative part of our auto-following kart. We use it to achieve a stable distance between the auto-following kart and the leader kart, in our case, a cleaning robot.
Development History
The development of the PID controller and its associated simulation framework has been an iterative process, marked by continuous enhancement and refinement.
Initial Implementation
The journey began with a simple implementation of the traditional PID controller. The focus was on creating a basic class structure in C++ that encapsulated the PID logic, allowing for straightforward application in simulations.
Key Features:
- Basic PID calculations.
- Initial support for both Arduino and non-Arduino environments through conditional compilation.
Enhancements and Adaptive Features
As the initial implementation was tested, several limitations became apparent, particularly regarding responsiveness and stability. This prompted the incorporation of advanced features such as:
-
Adaptive Gain Adjustment: Inspired by the need for dynamic responsiveness, the proportional gain $K_p$ was made adaptable based on the error's rate of change. This enhancement allowed for better handling of transient behaviors in the system.
-
Adaptive Integral Term: To mitigate integral windup, the integral gain $K_i$ was adjusted based on observed error amplitude. This feature improved system stability during sustained error conditions.
Error History Management
To accurately compute the integral and derivative terms, a robust error history management system was implemented. This system utilized a linked list structure to store past error values and timestamps, enabling precise calculations for both the integral and derivative components. The integration of error history management was crucial for implementing more advanced tuning strategies, inspired by established control theory practices.
Additional Derivative Term
Recognizing the need for improved damping characteristics, an additional derivative term $K_{dd}$ was introduced. This term accounted for the acceleration of error changes, allowing the controller to anticipate and react to rapid fluctuations more effectively.
Testing and Simulation Framework
With the PID logic in place, attention turned to building a comprehensive simulation framework to test various PID configurations. The introduction of the Car class for simulating follower and leader vehicles added a practical context for evaluating PID performance in a dynamic environment.
- PID Testing Function: A dedicated function was developed to simulate the interaction between the leader and follower karts, providing insights into how different PID parameters affected system behavior.
Optimization and Multi-threading
To facilitate further exploration of PID parameter tuning, optimization functions were integrated. Utilizing multi-threading allowed for the concurrent evaluation of multiple parameter combinations, greatly enhancing the testing efficiency. The optimization framework also included mechanisms for result logging and progress tracking, making it easier to analyze and visualize outcomes.
PID.hpp
Overview
This header file implements a PID controller that includes traditional PID control logic along with enhancements such as adaptive gain adjustment, adaptive integral term, error history management, and additional derivative term. It is also designed to be compatible with both Arduino and non-Arduino environments.
Arduino and Non-Arduino Compatibility
The header is designed to be compatible with both Arduino and non-Arduino environments. This allows developers to use the same PID controller implementation across different platforms.
Conditional Compilation:
#ifdef ARDUINO
#include <Arduino.h>
#include <math.h>
#define to_string(a) String(a)
#else
#include <cmath>
#include <string>
#define String string
using namespace std;
#endif
Traditional PID Controller
The traditional PID controller is defined by the following equation:
$$u(t) = Kp e(t) + Ki \int0^t e(\tau) d\tau + Kd \frac{de(t)}{dt} $$
Where:
- $u(t)$ is the control output.
- $e(t)$ is the error at time $t$, defined as the difference between the desired value and the measured value.
- $Kp$, $Ki$, and $K_d$ are the proportional, integral, and derivative gains, respectively.
Adaptive Gain Adjustment
The proportional gain $K_p$ is dynamically adjusted based on the interaction between the current error $e(t)$ and its rate of change (derivative). This adjustment is designed to improve the responsiveness of the controller to changing conditions. The adaptation can be mathematically expressed as follows:
$$Kp' = Kp \times (1 \pm eDP_a)$$
Where:
- $eDP = e(t) \times \frac{de(t)}{dt}$
- $eDP_m$ is a threshold that determines when to adjust the proportional gain.
- $eDPa$ is the adjustment factor that scales the change in $Kp$ based on the error’s derivative.
The key idea behind this adjustment is to increase the proportional gain when the system is experiencing a large error combined with a significant rate of change, which could indicate an approaching overshoot or oscillation. Conversely, if the error is decreasing but remains large, it may reduce the proportional gain to prevent excessive correction, allowing the system to stabilize more smoothly.
Adaptive Integral Term
Integral control is essential for eliminating steady-state error; however, it can lead to issues like integral windup, where the integral term accumulates excessively during periods of sustained error. To mitigate this, the integral gain $K_i$ is adaptively computed as follows:
$$Ki = Kp \times rTiM \times ap$$
Where:
- $ap = e^{-\frac{\text{amp}}{\text{maxAmp}}}$
- $rTiM$ is a constant that relates to the desired integral time constant.
The variable $amp$ represents the maximum amplitude of error observed over a session. By using the exponential decay factor $ap$, the contribution of the integral term is diminished during periods of high error, effectively reducing the risk of windup. When the error is within an acceptable range, $ap$ approaches 1, allowing the integral term to contribute effectively to the control output.
Error History Management
Inspired by the Ziegler–Nichols tuning method, the implementation maintains a history of past errors and their timestamps to calculate the derivative and integral terms accurately. This history management is crucial for two main reasons:
- Deriving the Rate of Change: The derivative term requires knowledge of how fast the error is changing. By storing previous error values in a linked list, we can accurately compute the change in error over time ($\frac{de(t)}{dt}$):
$$ dv = \frac{e(t) - preE}{dt}$$
- Integral Calculation: The integral of the error is accumulated over time, enabling the controller to respond to persistent deviations from the setpoint:
$$ intg += e(t) \times dt$$
Using a linked list allows for efficient management of this error history. As new errors are added, older entries can be pruned to ensure that only relevant data is retained, which helps avoid excessive memory usage and keeps the calculations focused on the most recent behavior of the system.
Additional Derivative Term
In addition to the standard derivative term $Kd$, an additional derivative term $K{dd}$ is included in the PID controller. This term represents a second derivative component, which can be defined as follows:
$$K{dd} = Kd \times TddM$$
Where:
- $TddM$ is a multiplier that adjusts the impact of the second derivative term.
The inclusion of $K_{dd}$ helps the controller anticipate changes in the error more effectively. By accounting for the acceleration of the error (i.e., how the rate of error change is itself changing), this additional term enhances the damping characteristics of the controller, allowing for more refined control responses, especially in systems that are subject to rapid changes or oscillations.
Code Structure
Data Structures
- Data: Holds the time difference (
dt) and error value (e). - Node: Represents an element in the linked list, containing
Dataand a pointer to the previous node. - List: Manages the linked list, holding pointers to the head and tail nodes and the size of the list.
PID Class
The PID class encapsulates the PID control logic. It includes:
-
Member Variables:
-
Control gains:
Kp,Ki,Kd,Kdd(additional derivative term). -
State variables:
preE,preDv,intg,preOut, andpreTfor storing previous error, derivative, integral term, last output, and last timestamp, respectively. -
dtXs: An instance ofListto maintain historical error data. -
Constructor: Initializes the PID parameters and state variables.
-
Update Method:
-
Takes the current error and timestamp, computes the control output, and updates the internal state.
-
Implements logic for calculating the adaptive gains, managing error history, and generating the control signal.
testCar.hpp
Overview
This header file includes a Car class for simulating the motion of a car, a PID controller for adjusting the speed of an auto-following kart based on the distance to a leader car, and several utility functions for data processing and results management.
Car Class
The Car class represents a simple vehicle model with basic motion dynamics. It includes attributes for speed and position, as well as methods to update these attributes.
PID Debugging Function
The pidDebug function is a utility for logging messages related to PID control operations.
Sum of Last Squared Values
bool write_results(const vector<pidTest> &data, const string &filename)Writes the results of PID tests to a CSV file for analysis.
ParameterRange
A structure to define the range of parameters for optimization.
- Attributes:
double start: The starting value of the parameter range.double end: The ending value of the parameter range.double step: The increment step for the parameter.
optimization.hpp
This header file defines the optimization functions and structures used to optimize the parameters of a PID controller in a car simulation without multi-threaded processing.
int optimize(vector<ParameterRange> ranges)
Optimizes the parameters of the PID controller based on the provided ranges and writes results to a CSV file.
-
Parameters:
-
vector<ParameterRange> ranges: A vector containing the ranges of parameters to optimize. -
Returns:
-
int: ReturnsEXIT_SUCCESS(0) if optimization completes successfully, otherwise returnsEXIT_FAILURE.
optimization_multithread.hpp
This header file defines the optimization functions and structures used to optimize the parameters of a PID controller in a car simulation with multi-threaded processing.
int optimize(vector<ParameterRange> ranges)
Optimizes the parameters of the PID controller based on the provided ranges and writes results to a CSV file.
-
Parameters:
-
vector<ParameterRange> ranges: A vector containing the ranges of parameters to optimize. -
Returns:
-
int: ReturnsEXIT_SUCCESS(0) if optimization completes successfully, otherwise returnsEXIT_FAILURE.
main.cpp
The code file demonstrates the usage of the optimization.hpp (or optimization_multithread.hpp) header file to perform parameter optimization for a PID controller. It defines a set of parameter ranges and calls the optimize function to evaluate different combinations of parameters.
Note that steps can't be zero or the program won't stop.
Wooden Board
The wooden board serves as the support structure of the auto-following kart that connects and props the components such as an Arduino board, an Omni wheel, a breadboard, 2 wheels, and 2 motors.
Afterthoughts
Looking back on the development and implementation of the Arduino-based PID auto-following kart, this project has been a remarkable journey that deepened my understanding of control systems, robotics, and the intricacies of sensor integration. As with any engineering project, the process involved a series of trials, errors, and discoveries, each contributing to a more refined and successful final result. Below are some detailed afterthoughts from different aspects of the project.
PID Tuning Challenges
One of the most insightful yet challenging parts of the project was tuning the PID (Proportional, Integral, Derivative) controller. At the outset, the kart’s behavior was far from ideal, with erratic turns, oscillations, and overshooting, even when the line-following task seemed straightforward. I initially faced difficulties because PID controllers require careful balancing of the three parameters to achieve smooth and stable control. After experimenting with different values, I came to realize that small adjustments in one parameter could drastically alter the kart’s performance. For example, increasing the proportional gain (Kp) made the kart more responsive but also introduced instability, while adjusting the derivative gain (Kd) helped reduce overshooting but didn’t entirely eliminate the oscillations. This hands-on experience taught me the delicate art of PID tuning and how to fine-tune it for real-time performance. It also provided me with a deeper appreciation for the role of feedback control in automation systems.
Sensor Calibration and Integration
The quality and accuracy of the sensors played a pivotal role in the kart's ability to follow the line. I opted to use infrared sensors for line detection, which are widely used for such applications due to their simplicity and affordability. However, I quickly learned that infrared sensors are sensitive to external factors like ambient light, surface texture, and even the angle at which they are positioned. Calibration became a crucial part of the setup process. Initially, the kart struggled to detect the line in areas with poor contrast or variable lighting, leading to misalignments and failures to follow the path.
By carefully adjusting the sensor threshold values and performing several test runs, I was able to achieve consistent detection in a variety of conditions. This process made me realize that sensor calibration is often as important as the controller itself and cannot be overlooked. I also discovered that the placement of sensors relative to the track—especially in ensuring that they cover a sufficient area and are aligned properly—was critical for reliable tracking. In hindsight, experimenting with different types of sensors (like color or ultrasonic sensors) might have improved performance in certain conditions, but the infrared sensors performed adequately for this project’s scale.
System Design and Component Selection
The design and selection of components had a significant impact on both the performance and scalability of the project. The kart required careful integration of the Arduino microcontroller with the motor driver, sensors, and power supply. Initially, I used a basic motor driver and simple infrared sensors, which worked for the task at hand, but I later realized there were opportunities for improvement. For example, a more efficient motor driver could have reduced the power losses during operation, and using sensors with better resolution would have increased accuracy.
Looking back, I could have made the design more compact by optimizing the wiring layout and minimizing the size of the motor driver and power supply. The kart's frame was designed to be large enough to house the components comfortably, but in future iterations, I would explore creating a more streamlined design, potentially using a custom PCB to reduce wiring complexity and improve reliability.
Moreover, I learned that a well-planned mechanical design, especially when integrating motors, wheels, and sensors, is crucial for minimizing friction and ensuring smooth operation. If I were to repeat this project, I would spend more time on optimizing the physical layout for better maneuverability and sensor coverage.
Power Management and Battery Considerations
The kart’s power management system was another crucial aspect that affected its overall performance. The kart was powered by a small rechargeable battery, and at times, the kart exhibited strange behavior when the battery voltage dropped too low. I soon realized that even a small voltage drop could cause the motor speed to fluctuate, leading to uneven turns and unpredictable movement. This issue underscored the importance of not only selecting the right power supply but also designing a system that accounts for power fluctuations.
In future iterations, I would focus on selecting a more reliable and higher-capacity battery, as well as incorporating a voltage regulator to ensure a stable supply of power to both the motors and the Arduino. Additionally, I would experiment with power-efficient motor controllers and explore the possibility of integrating a power management circuit to extend the kart’s runtime and prevent sudden drops in performance.
Real-World Performance vs. Simulated Testing
One of the most significant lessons from this project was the difference between controlled simulations and real-world performance. While the kart worked well on paper and in ideal conditions, the real world is much more unpredictable. Minor changes in track conditions—such as different surface textures, angles, or even lighting—could significantly impact the kart's performance. The kart sometimes failed to follow the line in certain situations, requiring me to tweak the PID settings and sensor calibration multiple times.
This experience highlighted the importance of robust real-world testing and the need for adaptive systems that can account for such variables. While the PID controller performed well under most circumstances, incorporating more advanced algorithms or adding feedback from additional sensors (e.g., accelerometers or gyroscopes) could help the kart adapt more dynamically to environmental changes.
Next Steps and Potential Improvements
Looking ahead, there are several avenues for improving the project. One major area to explore is enhancing the control algorithm itself. While the PID controller served its purpose, I believe more advanced methods, such as Model Predictive Control (MPC), could offer better performance in terms of accuracy and adaptability. MPC would allow the kart to predict future positions and adjust its speed and direction proactively, rather than reacting to changes after they occur.
In terms of hardware, upgrading the sensor suite would be beneficial. Using more sophisticated sensors, such as cameras for vision-based tracking or encoders for more accurate wheel position feedback, could significantly improve the kart’s performance. Implementing a camera-based line-following system could even enable the kart to follow more complex paths and handle obstacles.
Moreover, expanding the kart’s capabilities to handle more complex environments—such as navigating through obstacles, responding to dynamic changes in the track, or even following multiple lines—would be an exciting challenge. Adding wireless communication (such as Bluetooth or Wi-Fi) could allow for remote monitoring and control of the kart, opening the door to further experimentation with autonomous navigation.
Conclusion
In conclusion, the Arduino PID auto-following kart project has been a valuable learning experience that provided deep insights into feedback control systems, sensor integration, and the challenges of real-world robotics. While there were several hurdles along the way—especially in tuning the PID controller and dealing with power management—these challenges were integral to the learning process. The project has sparked my interest in robotics and control theory, and I am excited about the potential for future improvements. Ultimately, this experience has solidified my understanding of the complexities involved in building autonomous systems and has given me the confidence to tackle more advanced projects in the future.