├── .gitignore ├── Arduino ├── .gitignore ├── Components │ ├── ArduinoUno.mo │ ├── BodyShape.mo │ ├── DFLG6DOF.mo │ ├── DFlipFlop.mo │ ├── IC74HC595N4.mo │ ├── LED.mo │ ├── LEDMatrix8x8.mo │ ├── PiezoSpeaker.mo │ ├── ResettableDFlipFlop.mo │ ├── SEN136B5B.mo │ ├── Servo.mo │ ├── package.mo │ └── package.order ├── Examples │ ├── AnalogReadSerial.mo │ ├── BarGraph.mo │ ├── Blink.mo │ ├── BlinkWithoutDelay.mo │ ├── Button.mo │ ├── ControlledDCMotor.mo │ ├── Fade.mo │ ├── LEDMatrix.mo │ ├── Ping.mo │ ├── RobotArm.mo │ ├── ShiftOutHelloWorld.mo │ ├── Sweep.mo │ ├── ToneMelody.mo │ ├── package.mo │ └── package.order ├── Firmata │ ├── AnalogInput.mo │ ├── DigitalInput.mo │ ├── DigitalOutput.mo │ ├── Examples │ │ ├── AnalogInput.mo │ │ ├── Blink.mo │ │ ├── Button.mo │ │ ├── Fade.mo │ │ ├── Sweep.mo │ │ ├── package.mo │ │ └── package.order │ ├── Internal │ │ ├── BoardConnector.mo │ │ ├── ExternalFirmata.mo │ │ ├── PinConnector.mo │ │ ├── getBoardId.mo │ │ ├── numReceivedMessages.mo │ │ ├── numSentMessages.mo │ │ ├── package.mo │ │ ├── package.order │ │ ├── readAnalogPin.mo │ │ ├── readDigitalPin.mo │ │ ├── updateBoard.mo │ │ ├── writeAnalogPin.mo │ │ ├── writeDigitalPin.mo │ │ └── writeServoPin.mo │ ├── PWMOutput.mo │ ├── Port.mo │ ├── Servo.mo │ ├── package.mo │ └── package.order ├── Internal │ ├── ExternalArduino.mo │ ├── ModelicaFunctions.mo │ ├── PinDriver.mo │ ├── VariableBooleanPulse.mo │ ├── WavRecorder.mo │ ├── buildSketch.mo │ ├── buildSketchOM.mo │ ├── clean.mo │ ├── package.mo │ └── package.order ├── Resources │ ├── Images │ │ ├── AnalogRead_bb.png │ │ ├── Blink_bb.png │ │ ├── Button_bb.png │ │ ├── Fade_bb.png │ │ ├── Servo_bb.png │ │ ├── blink_example.png │ │ ├── blink_example_om.png │ │ └── simple.css │ ├── Include │ │ ├── ModelicaArduino.h │ │ ├── ModelicaUtilityFunctions.c │ │ ├── ModelicaUtilityFunctions.h │ │ └── WavRecorder.c │ ├── Library │ │ └── win64 │ │ │ └── .gitignore │ ├── Shapes │ │ ├── I_3DPart028615__2.png │ │ ├── I_3DPart028615__2.wrl │ │ ├── I_Case4Servo__10.png │ │ ├── I_Case4Servo__10.wrl │ │ ├── I_Gripper_Came_4Servo__31.png │ │ ├── I_Gripper_Came_4Servo__31.wrl │ │ ├── I_Gripper_Connecting_Rod_4Servo__32.png │ │ ├── I_Gripper_Connecting_Rod_4Servo__32.wrl │ │ ├── I_Gripper_Connecting_Rod_4Servo__35.png │ │ ├── I_Gripper_Connecting_Rod_4Servo__35.wrl │ │ ├── I_Gripper_Foam_4Servo__3.png │ │ ├── I_Gripper_Foam_4Servo__3.wrl │ │ ├── I_Gripper_Left_Arm_4Servo__33.png │ │ ├── I_Gripper_Left_Arm_4Servo__33.wrl │ │ ├── I_Gripper_Main_4Servo__29.png │ │ ├── I_Gripper_Main_4Servo__29.wrl │ │ ├── I_Gripper_Right_Arm_4Servo__34.png │ │ ├── I_Gripper_Right_Arm_4Servo__34.wrl │ │ ├── I_L4Servo__21.png │ │ ├── I_L4Servo__21.wrl │ │ ├── I_Rotative_Base_Main_4Servo__7.png │ │ ├── I_Rotative_Base_Main_4Servo__7.wrl │ │ ├── I_Rotative_Base_Top_4Servo__9.png │ │ ├── I_Rotative_Base_Top_4Servo__9.wrl │ │ ├── I_Servo_Horn__12.png │ │ ├── I_Servo_Horn__12.wrl │ │ ├── I_Servo__8.png │ │ ├── I_Servo__8.wrl │ │ ├── I_U4Servo__15.png │ │ ├── I_U4Servo__15.wrl │ │ └── RoboticArm_VRML_icon.png │ ├── Sketches │ │ ├── AnalogReadSerial.ino │ │ ├── AnalogWrite.ino │ │ ├── BarGraph.ino │ │ ├── Blink.ino │ │ ├── BlinkWithInterrupt.ino │ │ ├── BlinkWithoutDelay.ino │ │ ├── Button.ino │ │ ├── ControlledDCMotor.ino │ │ ├── Fade.ino │ │ ├── Functions.ino │ │ ├── LEDMatrix.ino │ │ ├── Melody.ino │ │ ├── Ping.ino │ │ ├── PulseInExample.ino │ │ ├── RobotArmGrabSponge.ino │ │ ├── ShftOut11.ino │ │ ├── Sweep.ino │ │ └── pitches.h │ └── Source │ │ ├── Arduino │ │ ├── Arduino.h │ │ ├── CMakeLists.txt │ │ ├── DFLG6DOF.h │ │ ├── Event.h │ │ ├── ModelicaArduino.cpp │ │ ├── ModelicaUtilities.cpp │ │ ├── ModelicaUtilities.h │ │ ├── PID_v1.cpp │ │ ├── PID_v1.h │ │ ├── Print.cpp │ │ ├── Print.h │ │ ├── Servo.cpp │ │ ├── Servo.h │ │ ├── Sketch.cpp │ │ ├── Sketch.h │ │ ├── SoftArduino.cpp │ │ ├── SoftArduino.h │ │ ├── SoftSerial.h │ │ ├── WCharacter.h │ │ ├── WMath.cpp │ │ ├── binary.h │ │ ├── inttypes.h │ │ ├── pins_arduino.h │ │ └── stdbool.h │ │ └── Firmata │ │ ├── CMakeLists.txt │ │ ├── FirmataConnection.cpp │ │ ├── FirmataConnection.h │ │ ├── ModelicaFirmata.cpp │ │ ├── ModelicaFirmata.h │ │ ├── serial.cpp │ │ └── serial.h ├── package.mo └── package.order ├── LICENSE.txt ├── README.md ├── dist.py └── tests ├── conftest.py ├── results ├── Arduino.Examples.AnalogReadSerial.mat ├── Arduino.Examples.BarGraph.mat ├── Arduino.Examples.Blink.mat ├── Arduino.Examples.BlinkWithoutDelay.mat ├── Arduino.Examples.Button.mat ├── Arduino.Examples.ControlledDCMotor.mat ├── Arduino.Examples.Fade.mat ├── Arduino.Examples.Ping.mat ├── Arduino.Examples.RobotArm.mat ├── Arduino.Examples.ShiftOutHelloWorld.mat └── Arduino.Examples.Sweep.mat ├── test_dymola.py ├── test_open_modelica.py └── utils.py /.gitignore: -------------------------------------------------------------------------------- 1 | # distribution folder & files 2 | dist/ 3 | *.zip 4 | 5 | # generated files 6 | README.html 7 | 8 | # IDEA files 9 | .idea/ 10 | 11 | # Dymola files 12 | Arduino/*.txt 13 | Arduino/*.c 14 | Arduino/*.mat 15 | Arduino/*.exe 16 | Arduino/*.exp 17 | Arduino/*.png 18 | 19 | # build artefacts 20 | Arduino/Resources/Source/Arduino/build/ 21 | Arduino/Resources/Source/Arduino/Win32/ 22 | Arduino/Resources/Source/Arduino/x64/ 23 | Arduino/Resources/Source/Firmata/build/ 24 | Arduino/Resources/Source/Firmata/Win32/ 25 | Arduino/Resources/Source/Firmata/x64/ 26 | *.a 27 | *.o 28 | *.obj 29 | *.lib 30 | 31 | # Temp folder 32 | Temp/ 33 | 34 | # Windows icon cache files 35 | Thumbs.db 36 | 37 | # test artifacts 38 | tests/work/ 39 | __pycache__ 40 | -------------------------------------------------------------------------------- /Arduino/.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Dymola 3 | *.bak-mo 4 | 5 | *.lib 6 | *.obj 7 | -------------------------------------------------------------------------------- /Arduino/Components/BodyShape.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Components; 2 | model BodyShape 3 | Modelica.Mechanics.MultiBody.Visualizers.Advanced.Shape shape( 4 | shapeType=shapeType, r=fixedRotation.frame_b.r_0, 5 | R=fixedRotation.frame_b.R, 6 | color=color, 7 | specularCoefficient=0.1) 8 | annotation (Placement(transformation(extent={{-10,-12},{10,8}}))); 9 | 10 | parameter Modelica.Mechanics.MultiBody.Types.ShapeType shapeType="modelica://Arduino/Resources/Shapes/I_Rotative_Base_Main_4Servo__7.wrl" 11 | "Type of shape (box, sphere, cylinder, pipecylinder, cone, pipe, beam, gearwheel, spring, )"; 12 | Modelica.Mechanics.MultiBody.Parts.FixedTranslation fixedTranslation(r=r, 13 | animation=false) 14 | annotation (Placement(transformation(extent={{-80,-10},{-60,10}}))); 15 | Modelica.Mechanics.MultiBody.Interfaces.Frame_a frame_a 16 | "Coordinate system fixed to the component with one cut-force and cut-torque" 17 | annotation (Placement(transformation(extent={{-116,-16},{-84,16}}))); 18 | parameter Modelica.Units.SI.Position r[3]={0,0,0} 19 | "Vector from frame_a to frame_b resolved in frame_a"; 20 | Modelica.Mechanics.MultiBody.Parts.FixedRotation fixedRotation( 21 | n={1,0,0}, 22 | angle=-90, 23 | rotationType=Modelica.Mechanics.MultiBody.Types.RotationTypes.PlanarRotationSequence, 24 | angles=angles, 25 | animation=false) 26 | annotation (Placement(transformation(extent={{-46,-10},{-26,10}}))); 27 | 28 | parameter Modelica.Units.NonSI.Angle_deg angles[3]={0,0,0} 29 | "Rotation angles around the axes defined in 'sequence'"; 30 | parameter Real color[3]={255,255,255} "Color of shape"; 31 | 32 | parameter String icon="modelica://Arduino/Resources/Shapes/I_Rotative_Base_Main_4Servo__7.png"; 33 | 34 | equation 35 | connect(fixedTranslation.frame_a, frame_a) annotation (Line( 36 | points={{-80,0},{-100,0}}, 37 | color={95,95,95}, 38 | thickness=0.5)); 39 | connect(fixedRotation.frame_a, fixedTranslation.frame_b) annotation (Line( 40 | points={{-46,0},{-60,0}}, 41 | color={95,95,95}, 42 | thickness=0.5)); 43 | annotation (Icon(coordinateSystem(preserveAspectRatio=false), graphics={ 44 | Rectangle( 45 | extent={{-100,100},{100,-100}}, 46 | lineColor={0,0,0}, 47 | fillColor=color, 48 | fillPattern=FillPattern.Solid), Bitmap(extent={{-100,-100},{100,100}}, 49 | fileName=icon)}), Diagram( 50 | coordinateSystem(preserveAspectRatio=false))); 51 | end BodyShape; 52 | -------------------------------------------------------------------------------- /Arduino/Components/DFlipFlop.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Components; 2 | model DFlipFlop 3 | extends Modelica.Blocks.Icons.PartialBooleanBlock; 4 | Modelica.Blocks.Interfaces.BooleanInput D "Data" 5 | annotation (Placement(transformation(extent={{-140,40},{-100,80}}), 6 | iconTransformation(extent={{-140,40},{-100,80}}))); 7 | Modelica.Blocks.Interfaces.BooleanInput C "Clock" annotation (Placement( 8 | transformation(extent={{-140,-80},{-100,-40}}), iconTransformation( 9 | extent={{-140,-80},{-100,-40}}))); 10 | Modelica.Blocks.Interfaces.BooleanOutput Q annotation (Placement( 11 | transformation(extent={{100,50},{120,70}}), iconTransformation(extent={{ 12 | 100,50},{120,70}}))); 13 | equation 14 | when edge(C) then 15 | Q = pre(D); 16 | end when; 17 | annotation (Line(points={{-120,60},{110,60}}, color={255,0,255}), 18 | Icon(coordinateSystem(preserveAspectRatio=false), graphics={ 19 | Text( 20 | extent={{-76,-30},{-36,-90}}, 21 | textString="C", 22 | textColor={0,0,0}), 23 | Text( 24 | extent={{44,88},{84,28}}, 25 | textString="Q"), 26 | Text( 27 | extent={{-78,90},{-38,30}}, 28 | textString="D", 29 | textColor={0,0,0})}), Diagram( 30 | coordinateSystem(preserveAspectRatio=false))); 31 | end DFlipFlop; 32 | -------------------------------------------------------------------------------- /Arduino/Components/LED.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Components; 2 | model LED "Light emitting diode with animation" 3 | extends Modelica.Electrical.Analog.Interfaces.TwoPin; 4 | Modelica.Electrical.Analog.Ideal.IdealDiode diode(Vknee=1) 5 | annotation (Placement(transformation(extent={{-40,-10},{-20,10}}))); 6 | Modelica.Blocks.Math.Mean mean(f=10) 7 | annotation (Placement(transformation(extent={{40,40},{60,60}}))); 8 | Modelica.Electrical.Analog.Sensors.CurrentSensor currentSensor 9 | annotation (Placement(transformation(extent={{20,10},{40,-10}}))); 10 | equation 11 | connect(diode.p, p) 12 | annotation (Line(points={{-40,0},{-100,0}}, color={0,0,255})); 13 | connect(diode.n,currentSensor. p) 14 | annotation (Line(points={{-20,0},{20,0}}, color={0,0,255})); 15 | connect(currentSensor.i,mean. u) annotation (Line(points={{30,11},{30,11},{30, 16 | 50},{38,50}}, color={0,0,127})); 17 | connect(currentSensor.n, n) 18 | annotation (Line(points={{40,0},{70,0},{100,0}}, color={0,0,255})); 19 | annotation (Icon(graphics={ 20 | Ellipse( 21 | extent={{-60,60},{60,-60}}, 22 | fillColor=DynamicSelect({100,40,40}, if mean.y > 0 then {100 + (mean.y 23 | /0.02)*155,40,40} else {100,40,40}), 24 | fillPattern=FillPattern.Solid, 25 | pattern=LinePattern.None), 26 | Polygon( 27 | points={{30,0},{-30,40},{-30,-40},{30,0}}, 28 | lineColor={0,0,0}), 29 | Line(points={{-90,0},{-30,0}},color={0,0,255}), 30 | Line(points={{30,0},{90,0}}, color={0,0,255}), 31 | Line(points={{30,40},{30,-40}}, color={0,0,255}), 32 | Line(points={{38,52},{58,72}}, color={28,108,200}), 33 | Polygon( 34 | points={{52,73},{59,73},{59,66},{52,73}}, 35 | lineColor={0,0,255}, 36 | fillPattern=FillPattern.Solid, 37 | fillColor={0,0,255}), 38 | Polygon( 39 | points={{68,59},{75,59},{75,52},{68,59}}, 40 | lineColor={0,0,255}, 41 | fillPattern=FillPattern.Solid, 42 | fillColor={0,0,255}), 43 | Line(points={{54,38},{74,58}}, color={28,108,200})})); 44 | end LED; 45 | -------------------------------------------------------------------------------- /Arduino/Components/PiezoSpeaker.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Components; 2 | model PiezoSpeaker "Piezo speaker that records its output to a WAV file" 3 | 4 | parameter Modelica.Units.SI.Period sampleInterval=1e-4 "Sample interval"; 5 | parameter String filename = "speaker.wav"; 6 | parameter Modelica.Units.SI.Voltage vRef = 5 "Reference voltage"; 7 | Arduino.Internal.WavRecorder wavRecorder= 8 | Arduino.Internal.WavRecorder(filename); 9 | 10 | Modelica.Electrical.Analog.Basic.Resistor resistor(R=1e3) annotation ( 11 | Placement(transformation(extent={{-10,10},{10,-10}}, rotation=270))); 12 | Modelica.Electrical.Analog.Interfaces.PositivePin p "Positive electrical pin" 13 | annotation (Placement(transformation(extent={{-110,30},{-90,50}}))); 14 | Modelica.Electrical.Analog.Interfaces.NegativePin n "Negative electrical pin" 15 | annotation (Placement(transformation(extent={{-110,-50},{-90,-30}}))); 16 | 17 | protected 18 | function sampleMe 19 | input Arduino.Internal.WavRecorder wavRecorder; 20 | input Modelica.Units.SI.Time timeIn; 21 | input Real value; 22 | external "C" WavRecorder_sample(wavRecorder, timeIn, value) annotation ( 23 | Include = "#include \"WavRecorder.c\"", 24 | IncludeDirectory="modelica://Arduino/Resources/Include"); 25 | end sampleMe; 26 | 27 | equation 28 | 29 | when sample(0, sampleInterval) then 30 | sampleMe(wavRecorder, time, (p.v - n.v) / vRef); 31 | end when; 32 | 33 | connect(resistor.p, p) annotation (Line(points={{1.77636e-15,10},{0,10},{0,40}, 34 | {-100,40}}, color={0,0,255})); 35 | connect(resistor.n, n) 36 | annotation (Line(points={{0,-10},{0,-40},{-100,-40}}, color={0,0,255})); 37 | annotation (Icon(coordinateSystem(preserveAspectRatio=false), graphics={ 38 | Rectangle( 39 | extent={{-40,60},{0,-60}}, 40 | lineColor={0,0,255}, 41 | fillColor={255,255,255}, 42 | fillPattern=FillPattern.Solid), 43 | Polygon( 44 | points={{0,60},{40,100},{40,-100},{0,-60},{0,60}}, 45 | lineColor={0,0,255}, 46 | fillColor={255,255,255}, 47 | fillPattern=FillPattern.Solid), 48 | Line(points={{-100,40},{-40,40}}, color={0,0,255}), 49 | Line(points={{-100,-40},{-40,-40}}, color={0,0,255}), 50 | Line(points={{-30,40},{-10,40}}, color={0,0,255}), 51 | Line(points={{-20,50},{-20,30}}, color={0,0,255}), 52 | Line(points={{-30,-40},{-10,-40}}, 53 | color={0,0,255})}), Diagram( 54 | coordinateSystem(preserveAspectRatio=false))); 55 | end PiezoSpeaker; 56 | -------------------------------------------------------------------------------- /Arduino/Components/ResettableDFlipFlop.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Components; 2 | model ResettableDFlipFlop 3 | extends Modelica.Blocks.Icons.PartialBooleanBlock; 4 | Modelica.Blocks.Interfaces.BooleanInput D "Data" 5 | annotation (Placement(transformation(extent={{-140,40},{-100,80}}), 6 | iconTransformation(extent={{-140,40},{-100,80}}))); 7 | Modelica.Blocks.Interfaces.BooleanInput C "Clock" annotation (Placement( 8 | transformation(extent={{-140,-80},{-100,-40}}), iconTransformation( 9 | extent={{-140,-80},{-100,-40}}))); 10 | Modelica.Blocks.Interfaces.BooleanOutput Q annotation (Placement( 11 | transformation(extent={{100,50},{120,70}}), iconTransformation(extent={{ 12 | 100,50},{120,70}}))); 13 | Modelica.Blocks.Interfaces.BooleanInput R "Reset" annotation (Placement( 14 | transformation( 15 | extent={{-20,-20},{20,20}}, 16 | rotation=90, 17 | origin={0,-120}), iconTransformation(extent={{-20,-20},{20,20}}, 18 | rotation=90, 19 | origin={0,-120}))); 20 | equation 21 | 22 | when {edge(C), edge(R)} then 23 | if R then 24 | Q = false; 25 | else 26 | Q = pre(D); 27 | end if; 28 | end when; 29 | 30 | annotation (Line(points={{-120,60},{110,60}}, color={255,0,255}), 31 | Icon(coordinateSystem(preserveAspectRatio=false), graphics={ 32 | Text( 33 | extent={{-90,-40},{-50,-80}}, 34 | textString="C", 35 | textColor={0,0,0}), 36 | Text( 37 | extent={{50,80},{90,40}}, 38 | textString="Q"), 39 | Text( 40 | extent={{-90,80},{-50,40}}, 41 | textString="D", 42 | textColor={0,0,0}), 43 | Text( 44 | extent={{-20,-50},{20,-90}}, 45 | textColor={0,0,0}, 46 | textString="R")}), Diagram( 47 | coordinateSystem(preserveAspectRatio=false))); 48 | end ResettableDFlipFlop; 49 | -------------------------------------------------------------------------------- /Arduino/Components/SEN136B5B.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Components; 2 | model SEN136B5B "SEN136B5B Ultrasonic Range Finder" 3 | Modelica.Electrical.Analog.Ideal.IdealTwoWaySwitch sensorGround5V 4 | "Ground if true, 5V if false" 5 | annotation (Placement(transformation(extent={{-90,40},{-70,60}}))); 6 | Modelica.Electrical.Analog.Basic.Resistor resistor(R=100) annotation ( 7 | Placement(transformation( 8 | extent={{-10,-10},{10,10}}, 9 | rotation=270, 10 | origin={-50,-40}))); 11 | Modelica.Electrical.Analog.Sensors.VoltageSensor voltageSensor annotation ( 12 | Placement(transformation( 13 | extent={{-10,10},{10,-10}}, 14 | rotation=270, 15 | origin={0,-50}))); 16 | Modelica.Blocks.Nonlinear.FixedDelay fixedDelay(delayTime=10e-6) 17 | annotation (Placement(transformation(extent={{20,-60},{40,-40}}))); 18 | Modelica.Blocks.Math.RealToBoolean realToBoolean(threshold=2.5) 19 | annotation (Placement(transformation(extent={{60,-60},{80,-40}}))); 20 | Modelica.Electrical.Analog.Interfaces.PositivePin pinSig 21 | annotation (Placement(transformation(extent={{-110,40},{-90,60}}), 22 | iconTransformation(extent={{-110,40},{-90,60}}))); 23 | Modelica.Electrical.Analog.Interfaces.PositivePin input5V 24 | "Positive pin (potential p.v > n.v for positive voltage drop v)" 25 | annotation (Placement(transformation(extent={{-110,-10},{-90,10}}), 26 | iconTransformation(extent={{-110,-10},{-90,10}}))); 27 | Modelica.Electrical.Analog.Interfaces.NegativePin ground "Negative pin" 28 | annotation (Placement(transformation(extent={{-110,-60},{-90,-40}}), 29 | iconTransformation(extent={{-110,-60},{-90,-40}}))); 30 | Modelica.Blocks.MathBoolean.FallingEdge startFallingEdge 31 | annotation (Placement(transformation(extent={{40,-20},{20,0}}))); 32 | Modelica.Blocks.Logical.RSFlipFlop 33 | rSFlipFlop 34 | annotation (Placement(transformation(extent={{0,40},{-20,20}}))); 35 | Modelica.Blocks.Logical.Timer timer 36 | annotation (Placement(transformation(extent={{-10,-10},{10,10}}, 37 | rotation=0, 38 | origin={30,70}))); 39 | Modelica.Blocks.Logical.Pre pre1 40 | annotation (Placement(transformation(extent={{-20,60},{0,80}}))); 41 | Modelica.Blocks.Logical.GreaterEqual greaterEqual annotation (Placement( 42 | transformation( 43 | extent={{10,-10},{-10,10}}, 44 | rotation=0, 45 | origin={30,30}))); 46 | Modelica.Blocks.Interfaces.RealInput distance annotation (Placement( 47 | transformation( 48 | extent={{-20,-20},{20,20}}, 49 | rotation=180, 50 | origin={120,0}))); 51 | Modelica.Blocks.Math.Gain metersToSeconds(k=2/340.29) 52 | annotation (Placement(transformation(extent={{80,12},{60,32}}))); 53 | equation 54 | connect(resistor.p,voltageSensor. p) 55 | annotation (Line(points={{-50,-30},{-50,-20},{1.77636e-015,-20},{ 56 | 1.77636e-015,-40}}, color={0,0,255})); 57 | connect(voltageSensor.n,resistor. n) annotation (Line(points={{0,-60},{-50, 58 | -60},{-50,-50}}, color={0,0,255})); 59 | connect(voltageSensor.v, fixedDelay.u) 60 | annotation (Line(points={{10,-50},{10,-50},{18,-50}}, 61 | color={0,0,127})); 62 | connect(fixedDelay.y, realToBoolean.u) 63 | annotation (Line(points={{41,-50},{50,-50},{58,-50}}, color={0,0,127})); 64 | connect(sensorGround5V.n1, resistor.p) annotation (Line(points={{-70,55},{-50, 65 | 55},{-50,-30}}, color={0,0,255})); 66 | connect(sensorGround5V.p, pinSig) 67 | annotation (Line(points={{-90,50},{-90,50},{-100,50}}, 68 | color={0,0,255})); 69 | connect(input5V, sensorGround5V.n2) annotation (Line(points={{-100,0},{-120,0}, 70 | {-60,0},{-60,50},{-70,50}}, color={0,0,255})); 71 | connect(timer.u,pre1. y) 72 | annotation (Line(points={{18,70},{18,70},{1,70}}, 73 | color={255,0,255})); 74 | connect(startFallingEdge.y, rSFlipFlop.S) annotation (Line(points={{18,-10},{ 75 | 18,-10},{10,-10},{10,24},{2,24}}, color={255,0,255})); 76 | connect(pre1.u, rSFlipFlop.Q) annotation (Line(points={{-22,70},{-30,70},{-30, 77 | 24},{-21,24}}, color={255,0,255})); 78 | connect(realToBoolean.y, startFallingEdge.u) annotation (Line(points={{81,-50}, 79 | {81,-50},{90,-50},{90,-10},{44,-10}}, color={255,0,255})); 80 | connect(timer.y, greaterEqual.u1) annotation (Line(points={{41,70},{60,70},{ 81 | 60,30},{42,30}}, color={0,0,127})); 82 | connect(metersToSeconds.u, distance) annotation (Line(points={{82,22},{90,22}, 83 | {90,0},{120,0}}, color={0,0,127})); 84 | connect(sensorGround5V.control, rSFlipFlop.Q) annotation (Line(points={{-80,58}, 85 | {-80,70},{-30,70},{-30,24},{-21,24}}, color={255,0,255})); 86 | connect(greaterEqual.u2, metersToSeconds.y) 87 | annotation (Line(points={{42,22},{59,22}}, color={0,0,127})); 88 | connect(rSFlipFlop.R, greaterEqual.y) annotation (Line(points={{2,36},{10,36}, 89 | {10,30},{19,30}}, color={255,0,255})); 90 | connect(ground, resistor.n) 91 | annotation (Line(points={{-100,-50},{-50,-50}}, color={0,0,255})); 92 | annotation (Icon(coordinateSystem(preserveAspectRatio=true, extent={{-100, 93 | -100},{100,100}}), graphics={ 94 | Rectangle( 95 | extent={{-60,100},{100,-100}}, 96 | lineColor={28,108,200}, 97 | fillColor={255,255,255}, 98 | fillPattern=FillPattern.Solid), 99 | Text( 100 | extent={{-52,-38},{20,-62}}, 101 | lineColor={28,108,200}, 102 | horizontalAlignment=TextAlignment.Left, 103 | textString="GND"), 104 | Text( 105 | extent={{-52,12},{6,-12}}, 106 | lineColor={28,108,200}, 107 | horizontalAlignment=TextAlignment.Left, 108 | textString="5V"), 109 | Text( 110 | extent={{-52,62},{16,38}}, 111 | lineColor={28,108,200}, 112 | textString="SIG", 113 | horizontalAlignment=TextAlignment.Left), 114 | Ellipse(extent={{20,80},{80,20}},lineColor={28,108,200}), 115 | Ellipse(extent={{20,-20},{80,-80}},lineColor={28,108,200}), 116 | Line(points={{-100,50},{-60,50}}, color={28,108,200}), 117 | Line(points={{-100,0},{-60,0}}, color={28,108,200}), 118 | Line(points={{-100,-50},{-60,-50}}, color={28,108,200})}), Diagram( 119 | coordinateSystem(preserveAspectRatio=false, extent={{-100,-100},{100, 120 | 100}}), graphics={ 121 | Text( 122 | extent={{-36,-78},{36,-88}}, 123 | lineColor={255,255,255}, 124 | fillColor={170,213,255}, 125 | fillPattern=FillPattern.Solid, 126 | textString="Sensor")})); 127 | end SEN136B5B; 128 | -------------------------------------------------------------------------------- /Arduino/Components/package.mo: -------------------------------------------------------------------------------- 1 | within Arduino; 2 | package Components "Library to simulate sketches on a virtual Arduino Uno" 3 | 4 | 5 | 6 | annotation (Icon(graphics={ 7 | Rectangle( 8 | lineColor={200,200,200}, 9 | fillColor={248,248,248}, 10 | fillPattern=FillPattern.HorizontalCylinder, 11 | extent={{-100,-100},{100,100}}, 12 | radius=25.0), 13 | Rectangle( 14 | lineColor={128,128,128}, 15 | extent={{-100.0,-100.0},{100.0,100.0}}, 16 | radius=25.0), 17 | Rectangle( 18 | origin={0,0}, 19 | fillColor={128,128,128}, 20 | fillPattern=FillPattern.Solid, 21 | extent={{-50,-60},{50,60}}, 22 | lineColor={80,80,80}), 23 | Line(points={{50,40},{80,40}}, color={0,0,0}), 24 | Line(points={{50,20},{80,20}}, color={0,0,0}), 25 | Line(points={{50,0},{80,0}}, color={0,0,0}), 26 | Line(points={{50,-20},{80,-20}}, color={0,0,0}), 27 | Line(points={{50,-40},{80,-40}}, color={0,0,0}), 28 | Line(points={{-74,40},{-50,40}}, color={0,0,0}), 29 | Line(points={{-74,20},{-50,20}}, color={0,0,0}), 30 | Line(points={{-74,0},{-50,0}}, color={0,0,0}), 31 | Line(points={{-74,-20},{-50,-20}}, color={0,0,0}), 32 | Line(points={{-74,-40},{-50,-40}}, color={0,0,0})})); 33 | end Components; 34 | -------------------------------------------------------------------------------- /Arduino/Components/package.order: -------------------------------------------------------------------------------- 1 | ArduinoUno 2 | SEN136B5B 3 | LED 4 | Servo 5 | BodyShape 6 | DFLG6DOF 7 | PiezoSpeaker 8 | IC74HC595N4 9 | DFlipFlop 10 | ResettableDFlipFlop 11 | LEDMatrix8x8 12 | -------------------------------------------------------------------------------- /Arduino/Examples/AnalogReadSerial.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Examples; 2 | model AnalogReadSerial "An Arduino reading a potentiometer" 3 | extends Modelica.Icons.Example; 4 | 5 | Components.ArduinoUno arduinoUno(sketch="AnalogReadSerial.ino") 6 | annotation (Placement(transformation(extent={{-20,-60},{60,40}}))); 7 | Modelica.Electrical.Analog.Basic.Potentiometer potentiometer(rConstant=0.01, 8 | useRinput=true) annotation (Placement(transformation( 9 | extent={{10,-10},{-10,10}}, 10 | rotation=90, 11 | origin={-80,0}))); 12 | Modelica.Blocks.Sources.Ramp ramp(duration=1) 13 | annotation (Placement(transformation( 14 | extent={{10,-10},{-10,10}}, 15 | rotation=0, 16 | origin={-50,10}))); 17 | Modelica.Electrical.Analog.Sources.ConstantVoltage constantVoltage(V=5) 18 | annotation (Placement(transformation( 19 | extent={{-10,-10},{10,10}}, 20 | rotation=270, 21 | origin={90,0}))); 22 | Modelica.Electrical.Analog.Basic.Ground ground 23 | annotation (Placement(transformation(extent={{10,-100},{30,-80}}))); 24 | equation 25 | connect(ramp.y, potentiometer.r) 26 | annotation (Line(points={{-61,10},{-69,10}}, color={0,0,127})); 27 | connect(potentiometer.contact, arduinoUno.A0) annotation (Line(points={{-70,-10}, 28 | {-20,-10},{-20,-12.5}}, color={0,0,255})); 29 | connect(ground.p, arduinoUno.GND) 30 | annotation (Line(points={{20,-80},{20,-60}}, color={0,0,255})); 31 | connect(constantVoltage.p, potentiometer.pin_p) annotation (Line(points={{90,10}, 32 | {90,60},{-80,60},{-80,10}}, color={0,0,255})); 33 | connect(arduinoUno.Vin, potentiometer.pin_p) annotation (Line(points={{20,40}, 34 | {20,60},{-80,60},{-80,10}}, color={0,0,255})); 35 | connect(constantVoltage.n, arduinoUno.GND) annotation (Line(points={{90,-10}, 36 | {90,-70},{20,-70},{20,-60}}, color={0,0,255})); 37 | connect(potentiometer.pin_n, arduinoUno.GND) annotation (Line(points={{-80, 38 | -10},{-80,-70},{20,-70},{20,-60}}, color={0,0,255})); 39 | annotation (Icon(coordinateSystem(preserveAspectRatio=false)), Diagram( 40 | coordinateSystem(preserveAspectRatio=false), graphics={Text( 41 | extent={{6,96},{96,84}}, 42 | lineColor={28,108,200}, 43 | textString="See https://www.arduino.cc/en/Tutorial/AnalogReadSerial 44 | for a description of the sketch and the circuit."), Rectangle(extent={{0,100},{ 45 | 100,80}}, lineColor={28,108,200})}), 46 | experiment); 47 | end AnalogReadSerial; 48 | -------------------------------------------------------------------------------- /Arduino/Examples/Blink.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Examples; 2 | model Blink "An Arduino with a blinking LED" 3 | extends Modelica.Icons.Example; 4 | 5 | Components.ArduinoUno arduinoUno(sketch="Blink.ino") 6 | annotation (Placement(transformation(extent={{-40,-50},{40,50}}, 7 | rotation=0, 8 | origin={0,-10}))); 9 | Modelica.Electrical.Analog.Basic.Ground ground 10 | annotation (Placement(transformation(extent={{-10,-100},{10,-80}}))); 11 | Modelica.Electrical.Analog.Sources.ConstantVoltage constantVoltage(V=5) 12 | annotation (Placement(transformation( 13 | extent={{-10,-10},{10,10}}, 14 | rotation=270, 15 | origin={-70,0}))); 16 | equation 17 | connect(constantVoltage.p, arduinoUno.Vin) annotation (Line(points={{-70,10}, 18 | {-70,50},{0,50},{0,40}}, color={0,0,255})); 19 | connect(arduinoUno.GND, ground.p) 20 | annotation (Line(points={{0,-60},{0,-80}}, color={0,0,255})); 21 | connect(constantVoltage.n, ground.p) annotation (Line(points={{-70,-10},{-70, 22 | -70},{0,-70},{0,-80}}, color={0,0,255})); 23 | annotation (Icon(coordinateSystem(preserveAspectRatio=false, extent={{-100, 24 | -100},{100,100}})), Diagram( 25 | coordinateSystem(preserveAspectRatio=false, extent={{-100,-100},{100, 26 | 100}}), graphics={Text( 27 | extent={{6,96},{94,82}}, 28 | lineColor={28,108,200}, 29 | textString="See https://www.arduino.cc/en/Tutorial/Blink 30 | for a description of the sketch and the circuit."), Rectangle(extent={{0,100},{ 31 | 100,80}}, lineColor={28,108,200})}), 32 | experiment(StopTime=10)); 33 | end Blink; 34 | -------------------------------------------------------------------------------- /Arduino/Examples/BlinkWithoutDelay.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Examples; 2 | model BlinkWithoutDelay "An Arduino with a blinking LED w/o using delay()" 3 | extends Modelica.Icons.Example; 4 | 5 | Components.ArduinoUno arduinoUno(sketch="BlinkWithoutDelay.ino") 6 | annotation (Placement(transformation(extent={{-40,-60},{40,40}}))); 7 | Modelica.Electrical.Analog.Sources.ConstantVoltage constantVoltage(V=5) 8 | annotation (Placement(transformation( 9 | extent={{-10,-10},{10,10}}, 10 | rotation=270, 11 | origin={-70,0}))); 12 | Modelica.Electrical.Analog.Basic.Ground ground 13 | annotation (Placement(transformation(extent={{-10,-100},{10,-80}}))); 14 | equation 15 | connect(constantVoltage.p, arduinoUno.Vin) annotation (Line(points={{-70,10}, 16 | {-70,50},{0,50},{0,40}}, color={0,0,255})); 17 | connect(constantVoltage.n, ground.p) annotation (Line(points={{-70,-10},{-70, 18 | -70},{0,-70},{0,-80}}, color={0,0,255})); 19 | connect(arduinoUno.GND, ground.p) 20 | annotation (Line(points={{0,-60},{0,-80}}, color={0,0,255})); 21 | annotation (Icon(coordinateSystem(preserveAspectRatio=false)), Diagram( 22 | coordinateSystem(preserveAspectRatio=false), graphics={Text( 23 | extent={{4,98},{96,82}}, 24 | lineColor={28,108,200}, 25 | textString="See https://www.arduino.cc/en/Tutorial/BlinkWithoutDelay 26 | for a description of the sketch and the circuit."), Rectangle(extent={{0,100},{ 27 | 100,80}}, lineColor={28,108,200})}), 28 | experiment(StopTime=10)); 29 | end BlinkWithoutDelay; 30 | -------------------------------------------------------------------------------- /Arduino/Examples/Button.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Examples; 2 | model Button "An Arduino connected to a button" 3 | extends Modelica.Icons.Example; 4 | 5 | Components.ArduinoUno arduinoUno(sketch="Button.ino") 6 | annotation (Placement(transformation(extent={{-60,-60},{20,40}}))); 7 | Modelica.Electrical.Analog.Basic.Resistor resistor(R=1e4) annotation ( 8 | Placement(transformation( 9 | extent={{-10,-10},{10,10}}, 10 | rotation=270, 11 | origin={40,-50}))); 12 | Modelica.Electrical.Analog.Ideal.IdealClosingSwitch idealClosingSwitch 13 | annotation (Placement(transformation( 14 | extent={{-10,-10},{10,10}}, 15 | rotation=270, 16 | origin={40,-10}))); 17 | Modelica.Blocks.Sources.BooleanPulse booleanPulse(width=25, period=1) 18 | annotation (Placement(transformation( 19 | extent={{10,-10},{-10,10}}, 20 | rotation=0, 21 | origin={70,-10}))); 22 | Modelica.Electrical.Analog.Basic.Ground ground 23 | annotation (Placement(transformation(extent={{-30,-100},{-10,-80}}))); 24 | Modelica.Electrical.Analog.Sources.ConstantVoltage constantVoltage(V=5) 25 | annotation (Placement(transformation( 26 | extent={{-10,-10},{10,10}}, 27 | rotation=270, 28 | origin={-80,0}))); 29 | equation 30 | connect(resistor.n, arduinoUno.GND) annotation (Line(points={{40,-60},{40,-70}, 31 | {-20,-70},{-20,-60}}, color={0,0,255})); 32 | connect(resistor.p, arduinoUno.D2) annotation (Line(points={{40,-40},{40, 33 | -32.5},{20,-32.5}}, color={0,0,255})); 34 | connect(idealClosingSwitch.n, arduinoUno.D2) annotation (Line(points={{40,-20}, 35 | {40,-32.5},{20,-32.5}}, color={0,0,255})); 36 | connect(booleanPulse.y, idealClosingSwitch.control) 37 | annotation (Line(points={{59,-10},{47,-10}}, 38 | color={255,0,255})); 39 | connect(ground.p, arduinoUno.GND) 40 | annotation (Line(points={{-20,-80},{-20,-60}}, color={0,0,255})); 41 | connect(constantVoltage.n, arduinoUno.GND) annotation (Line(points={{-80,-10}, 42 | {-80,-70},{-20,-70},{-20,-60}}, color={0,0,255})); 43 | connect(arduinoUno.Vin, idealClosingSwitch.p) annotation (Line(points={{-20,40}, 44 | {-20,60},{40,60},{40,0}}, color={0,0,255})); 45 | connect(constantVoltage.p, idealClosingSwitch.p) annotation (Line(points={{-80,10}, 46 | {-80,60},{40,60},{40,0}}, color={0,0,255})); 47 | annotation (Icon(coordinateSystem(preserveAspectRatio=false)), Diagram( 48 | coordinateSystem(preserveAspectRatio=false), graphics={Text( 49 | extent={{6,98},{94,82}}, 50 | lineColor={28,108,200}, 51 | textString="See https://www.arduino.cc/en/Tutorial/Button 52 | for a description of the sketch and the circuit."), Rectangle(extent={{0,100},{ 53 | 100,80}}, lineColor={28,108,200})}), 54 | experiment(StopTime=10)); 55 | end Button; 56 | -------------------------------------------------------------------------------- /Arduino/Examples/Fade.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Examples; 2 | model Fade "An Arduino dimming an LED" 3 | extends Modelica.Icons.Example; 4 | 5 | Components.ArduinoUno arduinoUno(sketch="Fade.ino") 6 | annotation (Placement(transformation(extent={{-40,-40},{40,60}}))); 7 | Modelica.Electrical.Analog.Basic.Resistor resistor(R=220) annotation ( 8 | Placement(transformation( 9 | extent={{-10,-10},{10,10}}, 10 | rotation=270, 11 | origin={60,-10}))); 12 | Components.LED LED annotation (Placement(transformation( 13 | extent={{-10,-10},{10,10}}, 14 | rotation=270, 15 | origin={60,-40}))); 16 | Modelica.Blocks.Sources.Sine sine( 17 | amplitude=0.5, 18 | f=0.5, 19 | offset=0.5) 20 | annotation (Placement(transformation(extent={{-54,14},{-66,26}}))); 21 | Modelica.Electrical.Analog.Basic.Potentiometer potentiometer(rConstant=0.01, 22 | useRinput=true) annotation (Placement(transformation( 23 | extent={{10,-10},{-10,10}}, 24 | rotation=90, 25 | origin={-88,10}))); 26 | Modelica.Electrical.Analog.Sources.ConstantVoltage constantVoltage(V=5) 27 | annotation (Placement(transformation( 28 | extent={{-10,-10},{10,10}}, 29 | rotation=270, 30 | origin={90,10}))); 31 | Modelica.Electrical.Analog.Basic.Ground ground 32 | annotation (Placement(transformation(extent={{-10,-100},{10,-80}}))); 33 | equation 34 | connect(resistor.p, arduinoUno.D9) annotation (Line(points={{60,0},{60,22.5}, 35 | {40,22.5}}, color={0,0,255})); 36 | connect(LED.p, resistor.n) 37 | annotation (Line(points={{60,-30},{60,-20}}, color={0,0,255})); 38 | connect(LED.n, arduinoUno.GND) annotation (Line(points={{60,-50},{60,-70},{0, 39 | -70},{0,-40}}, color={0,0,255})); 40 | connect(arduinoUno.A0, potentiometer.contact) annotation (Line(points={{-40,7.5}, 41 | {-58.875,7.5},{-58.875,0},{-78,0}}, color={0, 42 | 0,255})); 43 | connect(potentiometer.pin_n, arduinoUno.GND) annotation (Line(points={{-88,0}, 44 | {-88,-70},{0,-70},{0,-40}}, color={0,0,255})); 45 | connect(constantVoltage.p, arduinoUno.Vin) 46 | annotation (Line(points={{90,20},{90,70},{0,70},{0,60}}, color={0,0,255})); 47 | connect(constantVoltage.n, arduinoUno.GND) annotation (Line(points={{90,0},{ 48 | 90,-70},{0,-70},{0,-40}}, color={0,0,255})); 49 | connect(ground.p, arduinoUno.GND) 50 | annotation (Line(points={{0,-80},{0,-40}}, color={0,0,255})); 51 | connect(potentiometer.pin_p, arduinoUno.Vin) annotation (Line(points={{-88,20}, 52 | {-88,70},{0,70},{0,60}}, color={0,0,255})); 53 | connect(sine.y, potentiometer.r) 54 | annotation (Line(points={{-66.6,20},{-76,20}}, color={0,0,127})); 55 | annotation (Icon(coordinateSystem(preserveAspectRatio=false, extent={{-100, 56 | -100},{100,80}})), Diagram( 57 | coordinateSystem(preserveAspectRatio=false, extent={{-100,-100},{100, 58 | 100}}), graphics={Text( 59 | extent={{6,98},{94,82}}, 60 | lineColor={28,108,200}, 61 | textString="See https://www.arduino.cc/en/Tutorial/Fade 62 | for a description of the sketch and the circuit."), Rectangle(extent={{0,100},{ 63 | 100,80}}, lineColor={28,108,200})}), 64 | experiment(StopTime=10)); 65 | end Fade; 66 | -------------------------------------------------------------------------------- /Arduino/Examples/LEDMatrix.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Examples; 2 | model LEDMatrix "An Arduino connected to a button" 3 | extends Modelica.Icons.Example; 4 | 5 | Components.ArduinoUno arduinoUno(sampleInterval=0.01, sketch="LEDMatrix.ino") 6 | annotation (Placement(transformation(extent={{-40,-50},{40,50}}, 7 | rotation=90, 8 | origin={-20,-6}))); 9 | Modelica.Electrical.Analog.Basic.Ground ground 10 | annotation (Placement(transformation(extent={{-10,-10},{10,10}}, 11 | rotation=90, 12 | origin={50,90}))); 13 | Modelica.Electrical.Analog.Sources.ConstantVoltage constantVoltage(V=5) 14 | annotation (Placement(transformation( 15 | extent={{-10,-10},{10,10}}, 16 | rotation=0, 17 | origin={-18,90}))); 18 | Components.LEDMatrix8x8 lEDMatrix8x82_1 19 | annotation (Placement(transformation(extent={{46,-30},{94,18}}))); 20 | equation 21 | connect(lEDMatrix8x82_1.R8, arduinoUno.A5) annotation (Line(points={{72,-30}, 22 | {72,-80},{7.5,-80},{7.5,-46}}, color={0,0,255})); 23 | connect(lEDMatrix8x82_1.C5, arduinoUno.A4) annotation (Line(points={{76,-30}, 24 | {76,-86},{2.5,-86},{2.5,-46}}, color={0,0,255})); 25 | connect(lEDMatrix8x82_1.R6, arduinoUno.A3) annotation (Line(points={{80,-30}, 26 | {80,-92},{-2.5,-92},{-2.5,-46}}, color={0,0,255})); 27 | connect(lEDMatrix8x82_1.R3, arduinoUno.A2) annotation (Line(points={{84,-30}, 28 | {84,-98},{-7.5,-98},{-7.5,-46}}, color={0,0,255})); 29 | connect(constantVoltage.p, arduinoUno.Vin) 30 | annotation (Line(points={{-28,90},{-70,90},{-70,-6}}, color={0,0,255})); 31 | connect(arduinoUno.GND, constantVoltage.n) annotation (Line(points={{30,-6},{ 32 | 36,-6},{36,90},{-8,90}}, color={0,0,255})); 33 | connect(ground.p, constantVoltage.n) 34 | annotation (Line(points={{40,90},{-8,90}}, color={0,0,255})); 35 | connect(arduinoUno.D2, lEDMatrix8x82_1.C8) annotation (Line(points={{2.5,34}, 36 | {2,34},{2,40},{56,40},{56,18}}, color={0,0,255})); 37 | connect(arduinoUno.D3, lEDMatrix8x82_1.C7) annotation (Line(points={{-2.5,34}, 38 | {-2,34},{-2,44},{60,44},{60,18}}, color={0,0,255})); 39 | connect(arduinoUno.D4, lEDMatrix8x82_1.R2) annotation (Line(points={{-7.5,34}, 40 | {-6,34},{-6,48},{64,48},{64,18}}, color={0,0,255})); 41 | connect(arduinoUno.D5, lEDMatrix8x82_1.C1) annotation (Line(points={{-12.5,34}, 42 | {-12,34},{-12,52},{68,52},{68,18}}, color={0,0,255})); 43 | connect(arduinoUno.D6, lEDMatrix8x82_1.R4) annotation (Line(points={{-17.5,34}, 44 | {-16,34},{-16,56},{72,56},{72,18}}, color={0,0,255})); 45 | connect(arduinoUno.D7, lEDMatrix8x82_1.C6) annotation (Line(points={{-22.5,34}, 46 | {-24,34},{-24,60},{76,60},{76,18}}, color={0,0,255})); 47 | connect(arduinoUno.D8, lEDMatrix8x82_1.C4) annotation (Line(points={{-27.5,34}, 48 | {-27.5,64},{80,64},{80,18}}, color={0,0,255})); 49 | connect(arduinoUno.D9, lEDMatrix8x82_1.R1) annotation (Line(points={{-32.5,34}, 50 | {-32,34},{-32,68},{84,68},{84,18}}, color={0,0,255})); 51 | connect(lEDMatrix8x82_1.R5, arduinoUno.D10) annotation (Line(points={{56,-30}, 52 | {56,-56},{-96,-56},{-96,70},{-36,70},{-36,34},{-37.5,34}}, color={0,0, 53 | 255})); 54 | connect(lEDMatrix8x82_1.R7, arduinoUno.D11) annotation (Line(points={{60,-30}, 55 | {60,-62},{-90,-62},{-90,64},{-42,64},{-42,50},{-42.5,50},{-42.5,34}}, 56 | color={0,0,255})); 57 | connect(lEDMatrix8x82_1.C2, arduinoUno.D12) annotation (Line(points={{64,-30}, 58 | {64,-68},{-84,-68},{-84,58},{-47.5,58},{-47.5,34}}, color={0,0,255})); 59 | connect(lEDMatrix8x82_1.C3, arduinoUno.D13) annotation (Line(points={{68,-30}, 60 | {68,-74},{-78,-74},{-78,52},{-52.5,52},{-52.5,34}}, color={0,0,255})); 61 | annotation (Icon(coordinateSystem(preserveAspectRatio=false)), Diagram( 62 | coordinateSystem(preserveAspectRatio=false)), 63 | experiment(StopTime=5, __Dymola_Algorithm="Dassl")); 64 | end LEDMatrix; 65 | -------------------------------------------------------------------------------- /Arduino/Examples/Ping.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Examples; 2 | model Ping "An Arduino controlling an ultra-sonic range finder" 3 | extends Modelica.Icons.Example; 4 | 5 | Components.ArduinoUno arduinoUno(sampleInterval=1e-6, sketch="Ping.ino") 6 | annotation (Placement(transformation(extent={{-60,-50},{20,50}}))); 7 | Modelica.Blocks.Sources.Ramp ramp( 8 | duration=0.2, 9 | height=0.02, 10 | offset=0.01) annotation (Placement(transformation( 11 | extent={{6,-6},{-6,6}}, 12 | rotation=0, 13 | origin={90,-6}))); 14 | Components.SEN136B5B ultrasonicRangeFinder 15 | annotation (Placement(transformation(extent={{38,-22},{70,10}}))); 16 | Modelica.Electrical.Analog.Basic.Ground ground 17 | annotation (Placement(transformation(extent={{-30,-100},{-10,-80}}))); 18 | Modelica.Electrical.Analog.Sources.ConstantVoltage constantVoltage(V=5) 19 | annotation (Placement(transformation( 20 | extent={{-10,-10},{10,10}}, 21 | rotation=270, 22 | origin={-80,0}))); 23 | equation 24 | connect(ultrasonicRangeFinder.distance, ramp.y) 25 | annotation (Line(points={{73.2,-6},{83.4,-6}}, color={0,0,127})); 26 | connect(ultrasonicRangeFinder.ground, arduinoUno.GND) annotation (Line(points={{38,-14}, 27 | {38,-60},{-20,-60},{-20,-50}}, color={0,0,255})); 28 | connect(ultrasonicRangeFinder.pinSig, arduinoUno.D7) annotation (Line(points={{38,2},{ 29 | 20,2},{20,2.5}}, color={0,0, 30 | 255})); 31 | connect(constantVoltage.p, arduinoUno.Vin) annotation (Line(points={{-80,10}, 32 | {-80,60},{-20,60},{-20,50}}, color={0,0,255})); 33 | connect(ultrasonicRangeFinder.input5V, arduinoUno.Vin) annotation (Line( 34 | points={{38,-6},{30,-6},{30,60},{-20,60},{-20,50}}, color={0,0,255})); 35 | connect(ground.p, arduinoUno.GND) 36 | annotation (Line(points={{-20,-80},{-20,-50}}, color={0,0,255})); 37 | connect(constantVoltage.n, arduinoUno.GND) annotation (Line(points={{-80,-10}, 38 | {-80,-60},{-20,-60},{-20,-50}}, color={0,0,255})); 39 | annotation (Icon(coordinateSystem(preserveAspectRatio=false)), Diagram( 40 | coordinateSystem(preserveAspectRatio=false), graphics={Text( 41 | extent={{8,98},{92,84}}, 42 | lineColor={28,108,200}, 43 | textString="See https://www.arduino.cc/en/Tutorial/Ping 44 | for a description of the sketch and the circuit."), Rectangle(extent={{0,100},{ 45 | 100,80}}, lineColor={28,108,200})}), 46 | experiment(StopTime=0.21)); 47 | end Ping; 48 | -------------------------------------------------------------------------------- /Arduino/Examples/RobotArm.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Examples; 2 | model RobotArm 3 | "A 6 degree of freedom robot arm controlled by an Arduino" 4 | extends Modelica.Icons.Example; 5 | 6 | Components.ArduinoUno arduinoUno(sketch="RobotArmGrabSponge.ino") 7 | annotation (Placement(transformation(extent={{-40,-50},{40,50}}, 8 | rotation=0, 9 | origin={-30,0}))); 10 | Modelica.Electrical.Analog.Basic.Ground ground 11 | annotation (Placement(transformation(extent={{-40,-90},{-20,-70}}))); 12 | Modelica.Electrical.Analog.Sources.ConstantVoltage constantVoltage(V=5) 13 | annotation (Placement(transformation( 14 | extent={{-10,-10},{10,10}}, 15 | rotation=270, 16 | origin={-100,10}))); 17 | Components.DFLG6DOF robotArmWithServos 18 | annotation (Placement(transformation(extent={{60,-20},{100,20}}))); 19 | equation 20 | connect(constantVoltage.p, arduinoUno.Vin) annotation (Line(points={{-100,20}, 21 | {-100,60},{-30,60},{-30,50}}, 22 | color={0,0,255})); 23 | connect(arduinoUno.GND, ground.p) 24 | annotation (Line(points={{-30,-50},{-30,-70}}, 25 | color={0,0,255})); 26 | connect(constantVoltage.n, ground.p) annotation (Line(points={{-100,0},{-100,-60}, 27 | {-30,-60},{-30,-70}}, color={0,0,255})); 28 | connect(robotArmWithServos.GND1, ground.p) annotation (Line(points={{60,-15}, 29 | {44,-15},{44,-60},{-30,-60},{-30,-70}},color={0,0,255})); 30 | connect(robotArmWithServos.basePin, arduinoUno.D3) annotation (Line(points={{60,-10}, 31 | {40,-10},{40,-17.5},{10,-17.5}}, color={0,0,255})); 32 | connect(robotArmWithServos.shoulderPin, arduinoUno.D5) annotation (Line( 33 | points={{60,-5},{36,-5},{36,-7.5},{10,-7.5}}, color={0,0,255})); 34 | connect(robotArmWithServos.elbowPin, arduinoUno.D6) annotation (Line(points={{ 35 | 60,0},{32,0},{32,-2.5},{10,-2.5}}, color={0,0,255})); 36 | connect(robotArmWithServos.wristVerPin, arduinoUno.D9) annotation (Line( 37 | points={{60,5},{32,5},{32,12.5},{10,12.5}}, color={0,0,255})); 38 | connect(robotArmWithServos.wristRotPin, arduinoUno.D10) annotation (Line( 39 | points={{60,10},{36,10},{36,17.5},{10,17.5}}, color={0,0,255})); 40 | connect(robotArmWithServos.gripperPin, arduinoUno.D11) annotation (Line( 41 | points={{60.05,15},{40,15},{40,22.5},{10,22.5}}, color={0,0,255})); 42 | annotation ( 43 | Icon(coordinateSystem(preserveAspectRatio=false, extent={{-100,-100},{100, 44 | 100}})), 45 | Diagram(coordinateSystem(preserveAspectRatio=false, extent={{-120,-100},{ 46 | 120,100}})), 47 | experiment(StopTime=20)); 48 | end RobotArm; 49 | -------------------------------------------------------------------------------- /Arduino/Examples/Sweep.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Examples; 2 | model Sweep "An Arduino controlling a servo" 3 | extends Modelica.Icons.Example; 4 | 5 | Components.ArduinoUno arduinoUno(sketch="Sweep.ino") 6 | annotation (Placement(transformation(extent={{-40,-50},{40,50}}, 7 | rotation=0, 8 | origin={0,-10}))); 9 | Modelica.Electrical.Analog.Sources.ConstantVoltage constantVoltage(V=5) 10 | annotation (Placement(transformation( 11 | extent={{-10,-10},{10,10}}, 12 | rotation=270, 13 | origin={-80,0}))); 14 | Modelica.Electrical.Analog.Basic.Ground ground 15 | annotation (Placement(transformation(extent={{-10,-100},{10,-80}}))); 16 | Components.Servo servo 17 | annotation (Placement(transformation(extent={{60,-12},{80,8}}))); 18 | equation 19 | connect(ground.p, arduinoUno.GND) 20 | annotation (Line(points={{0,-80},{0,-60}}, color={0,0,255})); 21 | connect(constantVoltage.n, arduinoUno.GND) annotation (Line(points={{-80,-10}, 22 | {-80,-70},{0,-70},{0,-60}},color={0,0,255})); 23 | connect(constantVoltage.p, arduinoUno.Vin) 24 | annotation (Line(points={{-80,10},{-80,60},{0,60},{0,40}}, 25 | color={0,0,255})); 26 | connect(servo.SIG, arduinoUno.D9) annotation (Line(points={{60,2},{60,2.5},{ 27 | 40,2.5}}, color={0,0,255})); 28 | connect(servo.GND, arduinoUno.GND) annotation (Line(points={{60,-6},{60,-6},{ 29 | 60,-70},{0,-70},{0,-60}}, color={0,0,255})); 30 | annotation (Icon(coordinateSystem(preserveAspectRatio=false, extent={{-100, 31 | -100},{100,100}})), Diagram( 32 | coordinateSystem(preserveAspectRatio=false, extent={{-100,-100},{100, 33 | 100}}), graphics={Text( 34 | extent={{6,96},{94,82}}, 35 | lineColor={28,108,200}, 36 | textString="See https://www.arduino.cc/en/Tutorial/Sweep 37 | for a description of the sketch and the circuit."), Rectangle(extent={{0,100},{ 38 | 100,80}}, lineColor={28,108,200})}), 39 | experiment(StopTime=10)); 40 | end Sweep; 41 | -------------------------------------------------------------------------------- /Arduino/Examples/ToneMelody.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Examples; 2 | model ToneMelody "Play a Melody using the tone() function" 3 | extends Modelica.Icons.Example; 4 | 5 | Components.PiezoSpeaker piezoSpeaker(sampleInterval=1/sampleFrequency, 6 | filename="melody.wav") 7 | annotation (Placement(transformation(extent={{60,-10},{80,10}}))); 8 | Components.ArduinoUno arduinoUno(sampleInterval=1/sampleFrequency, 9 | sketch="Melody.ino") 10 | annotation (Placement(transformation(extent={{-40,-50},{40,50}}))); 11 | Modelica.Electrical.Analog.Sources.ConstantVoltage constantVoltage(V=5) 12 | annotation (Placement(transformation( 13 | extent={{-10,-10},{10,10}}, 14 | rotation=270, 15 | origin={-60,0}))); 16 | Modelica.Electrical.Analog.Basic.Ground ground 17 | annotation (Placement(transformation(extent={{-10,-100},{10,-80}}))); 18 | parameter Modelica.Units.SI.Frequency sampleFrequency=44100 "Sample frequency"; 19 | equation 20 | connect(constantVoltage.p,arduinoUno. Vin) annotation (Line(points={{-60,10},{ 21 | -60,60},{0,60},{0,50}}, color={0,0,255})); 22 | connect(ground.p,arduinoUno. GND) 23 | annotation (Line(points={{0,-80},{0,-50}}, color={0,0,255})); 24 | connect(constantVoltage.n,arduinoUno. GND) annotation (Line(points={{-60,-10}, 25 | {-60,-60},{0,-60},{0,-50}}, color={0,0,255})); 26 | connect(piezoSpeaker.n, arduinoUno.GND) annotation (Line(points={{60,-4},{52, 27 | -4},{52,-60},{0,-60},{0,-50}}, color={0,0,255})); 28 | connect(piezoSpeaker.p, arduinoUno.D8) annotation (Line(points={{60,4},{52,4}, 29 | {52,7.5},{40,7.5}}, color={0,0,255})); 30 | annotation (Icon(coordinateSystem(preserveAspectRatio=false)), Diagram( 31 | coordinateSystem(preserveAspectRatio=false), graphics={Text( 32 | extent={{8,98},{92,84}}, 33 | lineColor={28,108,200}, 34 | textString="See https://docs.arduino.cc/built-in-examples/digital/toneMelody 35 | for a description of the sketch and the circuit."), Rectangle(extent={{0,100},{ 36 | 100,80}}, lineColor={28,108,200})}), 37 | experiment(StopTime=2.5)); 38 | end ToneMelody; 39 | -------------------------------------------------------------------------------- /Arduino/Examples/package.mo: -------------------------------------------------------------------------------- 1 | within Arduino; 2 | package Examples "Examples that demonstrate the usage of the virtual Arduino Uno" 3 | extends Modelica.Icons.ExamplesPackage; 4 | end Examples; 5 | -------------------------------------------------------------------------------- /Arduino/Examples/package.order: -------------------------------------------------------------------------------- 1 | AnalogReadSerial 2 | BarGraph 3 | Blink 4 | BlinkWithoutDelay 5 | Button 6 | Fade 7 | Ping 8 | Sweep 9 | ControlledDCMotor 10 | RobotArm 11 | ToneMelody 12 | ShiftOutHelloWorld 13 | LEDMatrix 14 | -------------------------------------------------------------------------------- /Arduino/Firmata/AnalogInput.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata; 2 | model AnalogInput 3 | 4 | parameter Integer analogInputPin(min=0, max=5) "Analog input pin"; 5 | parameter Real initValue = 0 "Initial value"; 6 | parameter Real minValue = 0 "Minimum value"; 7 | parameter Real maxValue = 1 "Maximum value"; 8 | 9 | Internal.PinConnector pinConnector 10 | annotation (Placement(transformation(extent={{-110,-10},{-90,10}}), 11 | iconTransformation(extent={{-116,-16},{-84,16}}))); 12 | Modelica.Blocks.Interfaces.RealOutput y 13 | annotation (Placement(transformation(extent={{100,-10},{120,10}}))); 14 | equation 15 | 16 | when (sample(0, 0.001)) then 17 | y = Firmata.Internal.readAnalogPin( 18 | 15 + analogInputPin, 19 | minValue, 20 | maxValue, 21 | initValue, 22 | pinConnector); 23 | end when 24 | annotation (Icon(coordinateSystem(preserveAspectRatio=false), graphics={ 25 | Polygon( 26 | points={{-100,0},{-20,80},{100,80},{100,-80},{-20,-80},{-100,0}}, 27 | lineColor={0,0,0}, 28 | fillColor={255,255,255}, 29 | fillPattern=FillPattern.Solid), 30 | Line( 31 | points={{-78,66}}, 32 | color={0,0,0}, 33 | pattern=LinePattern.None), 34 | Line( 35 | points={{-40,0},{-10,40},{20,0},{50,-40},{80,0}}, 36 | color={28,108,200}, 37 | smooth=Smooth.Bezier), Text( 38 | extent={{-20,-38},{90,-72}}, 39 | lineColor={0,0,0}, 40 | textString="A%analogInputPin")}), 41 | Diagram(coordinateSystem( 42 | preserveAspectRatio=false)), 43 | experiment, 44 | __Dymola_experimentSetupOutput, 45 | __Dymola_experimentFlags( 46 | Advanced(GenerateVariableDependencies=false, OutputModelicaCode=false), 47 | Evaluate=false, 48 | OutputCPUtime=false, 49 | OutputFlatModelica=false)); 50 | 51 | end AnalogInput; 52 | -------------------------------------------------------------------------------- /Arduino/Firmata/DigitalInput.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata; 2 | model DigitalInput 3 | 4 | parameter Integer digitalPin(min=0, max=13) "Digital pin"; 5 | parameter Boolean initValue = false "Initial values"; 6 | 7 | Internal.PinConnector pinConnector 8 | annotation (Placement(transformation(extent={{-110,-10},{-90,10}}), 9 | iconTransformation(extent={{-116,-16},{-84,16}}))); 10 | Modelica.Blocks.Interfaces.BooleanOutput y 11 | annotation (Placement(transformation(extent={{100,-10},{120,10}}))); 12 | 13 | equation 14 | y = Firmata.Internal.readDigitalPin( 15 | digitalPin, 16 | initValue, 17 | pinConnector, 18 | integer(time)); 19 | 20 | annotation (Icon(coordinateSystem(preserveAspectRatio=false), graphics={ 21 | Polygon( 22 | points={{-100,0},{-20,80},{100,80},{100,-80},{-20,-80},{-100,0}}, 23 | lineColor={0,0,0}, 24 | fillColor={255,255,255}, 25 | fillPattern=FillPattern.Solid), 26 | Line(points={{-48,-16},{6,-16},{6,28},{48,28},{48,-16}, 27 | {76,-16}}, color={255,0,255}), 28 | Text( 29 | extent={{-20,-38},{90,-72}}, 30 | lineColor={0,0,0}, 31 | textString="D%digitalPin")}), Diagram(coordinateSystem( 32 | preserveAspectRatio=false))); 33 | end DigitalInput; 34 | -------------------------------------------------------------------------------- /Arduino/Firmata/DigitalOutput.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata; 2 | model DigitalOutput 3 | 4 | parameter Integer digitalPin(min=0, max=13) "Digital pin"; 5 | 6 | Internal.PinConnector pinConnector 7 | annotation (Placement(transformation(extent={{84,-16},{116,16}}))); 8 | Modelica.Blocks.Interfaces.BooleanInput value 9 | annotation (Placement(transformation(extent={{-140,-20},{-100,20}}))); 10 | 11 | equation 12 | Firmata.Internal.writeDigitalPin( 13 | digitalPin, 14 | pinConnector, 15 | value); 16 | 17 | annotation (Icon(coordinateSystem(preserveAspectRatio=false), graphics={ 18 | Polygon( 19 | points={{100,0},{20,80},{-100,80},{-100,-80},{20,-80},{100,0}}, 20 | lineColor={0,0,0}, 21 | fillColor={255,255,255}, 22 | fillPattern=FillPattern.Solid), 23 | Line(points={{-82,-16},{-22,-16},{-22,28},{20,28},{20, 24 | -16},{48,-16}}, color={255,0,255}), 25 | Text( 26 | extent={{-90,-36},{20,-70}}, 27 | lineColor={0,0,0}, 28 | textString="D%digitalPin")}), Diagram(coordinateSystem( 29 | preserveAspectRatio=false))); 30 | end DigitalOutput; 31 | -------------------------------------------------------------------------------- /Arduino/Firmata/Examples/AnalogInput.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Examples; 2 | model AnalogInput "Read a connected potentiometer" 3 | import Arduino.Firmata; 4 | extends Modelica.Icons.Example; 5 | 6 | Firmata.Port port 7 | annotation (Placement(transformation(extent={{30,-40},{10,-20}}))); 8 | Firmata.AnalogInput analogInput(analogInputPin=0) 9 | annotation (Placement(transformation(extent={{50,-40},{70,-20}}))); 10 | equation 11 | connect(port.boardConnector, analogInput.pinConnector) 12 | annotation (Line(points={{30,-30},{50,-30}}, color={0,0,0})); 13 | annotation ( 14 | Icon(coordinateSystem(preserveAspectRatio=false)), 15 | Diagram(coordinateSystem(preserveAspectRatio=false), graphics={Bitmap( 16 | extent={{-100,-56},{8,86}}, fileName= 17 | "modelica://Arduino/Resources/Images/AnalogRead_bb.png")}), 18 | experiment(StopTime=30)); 19 | end AnalogInput; 20 | -------------------------------------------------------------------------------- /Arduino/Firmata/Examples/Blink.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Examples; 2 | model Blink "Control the LED 'L' on a connected Arduino" 3 | extends Modelica.Icons.Example; 4 | 5 | Port port annotation (Placement(transformation(extent={{-24,0},{-4,20}}))); 6 | DigitalOutput digitalOutput(digitalPin=13) 7 | annotation (Placement(transformation(extent={{-54,0},{-34,20}}))); 8 | Modelica.Blocks.Sources.BooleanPulse booleanPulse(period=1, startTime=5) 9 | annotation (Placement(transformation(extent={{-90,0},{-70,20}}))); 10 | equation 11 | connect(booleanPulse.y, digitalOutput.value) 12 | annotation (Line(points={{-69,10},{-56,10}}, 13 | color={255,0,255})); 14 | connect(digitalOutput.pinConnector, port.boardConnector) 15 | annotation (Line(points={{-34,10},{-24,10}}, color={0,0,0})); 16 | annotation ( 17 | Icon(coordinateSystem(preserveAspectRatio=false)), 18 | Diagram(coordinateSystem(preserveAspectRatio=false), graphics={Bitmap( 19 | extent={{2,-58},{92,40}}, fileName= 20 | "modelica://Arduino/Resources/Images/Blink_bb.png")}), 21 | experiment(StopTime=30)); 22 | end Blink; 23 | -------------------------------------------------------------------------------- /Arduino/Firmata/Examples/Button.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Examples; 2 | model Button "Read a connected button" 3 | extends Modelica.Icons.Example; 4 | 5 | Port port annotation (Placement(transformation(extent={{30,-40},{10,-20}}))); 6 | DigitalInput digitalInput(digitalPin=2) 7 | annotation (Placement(transformation(extent={{50,-40},{70,-20}}))); 8 | equation 9 | connect(port.boardConnector, digitalInput.pinConnector) 10 | annotation (Line(points={{30,-30},{50,-30}}, color={0,0,0})); 11 | annotation ( 12 | Icon(coordinateSystem(preserveAspectRatio=false)), 13 | Diagram(coordinateSystem(preserveAspectRatio=false), graphics={Bitmap( 14 | extent={{-100,-56},{10,80}}, fileName= 15 | "modelica://Arduino/Resources/Images/Button_bb.png")}), 16 | experiment(StopTime=30)); 17 | end Button; 18 | -------------------------------------------------------------------------------- /Arduino/Firmata/Examples/Fade.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Examples; 2 | model Fade "Dim a connected LED" 3 | extends Modelica.Icons.Example; 4 | 5 | Port port annotation (Placement(transformation(extent={{-20,-20},{0,0}}))); 6 | PWMOutput pwmOutput(digitalPin=9) 7 | annotation (Placement(transformation(extent={{-52,-20},{-32,0}}))); 8 | Modelica.Blocks.Sources.Trapezoid trapezoid( 9 | rising=1, 10 | width=0, 11 | falling=1, 12 | period=2) 13 | annotation (Placement(transformation(extent={{-90,-20},{-70,0}}))); 14 | equation 15 | connect(pwmOutput.pinConnector, port.boardConnector) 16 | annotation (Line(points={{-32,-10},{-20,-10}}, color={0,0,0})); 17 | connect(trapezoid.y, pwmOutput.u) 18 | annotation (Line(points={{-69,-10},{-54,-10}}, color={0,0,127})); 19 | annotation ( 20 | Icon(coordinateSystem(preserveAspectRatio=false)), 21 | Diagram(coordinateSystem(preserveAspectRatio=false), graphics={Bitmap( 22 | extent={{2,-64},{96,76}}, fileName= 23 | "modelica://Arduino/Resources/Images/Fade_bb.png")}), 24 | experiment(StopTime=30)); 25 | end Fade; 26 | -------------------------------------------------------------------------------- /Arduino/Firmata/Examples/Sweep.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Examples; 2 | model Sweep "Control a connected servo" 3 | extends Modelica.Icons.Example; 4 | 5 | Port port annotation (Placement(transformation(extent={{-20,0},{0,20}}))); 6 | Servo servo(digitalPin=9) 7 | annotation (Placement(transformation(extent={{-52,0},{-32,20}}))); 8 | Modelica.Blocks.Sources.Sine sine( 9 | startTime=5, 10 | offset=0.5, 11 | amplitude=0.5, 12 | f=0.5) annotation (Placement(transformation(extent={{-90,0},{-70,20}}))); 13 | equation 14 | connect(servo.pinConnector, port.boardConnector) 15 | annotation (Line(points={{-32,10},{-20,10}}, color={0,0,0})); 16 | connect(sine.y, servo.u) 17 | annotation (Line(points={{-69,10},{-54,10}}, 18 | color={0,0,127})); 19 | annotation ( 20 | Icon(coordinateSystem(preserveAspectRatio=false)), 21 | Diagram(coordinateSystem(preserveAspectRatio=false), graphics={Bitmap( 22 | extent={{8,-52},{98,86}}, fileName= 23 | "modelica://Arduino/Resources/Images/Servo_bb.png")}), 24 | experiment(StopTime=30)); 25 | end Sweep; 26 | -------------------------------------------------------------------------------- /Arduino/Firmata/Examples/package.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata; 2 | package Examples "Examples that demonstrate the connection of models with real-world circuits using the Firmata protocol" 3 | extends Modelica.Icons.ExamplesPackage; 4 | 5 | 6 | 7 | 8 | 9 | end Examples; 10 | -------------------------------------------------------------------------------- /Arduino/Firmata/Examples/package.order: -------------------------------------------------------------------------------- 1 | Blink 2 | Fade 3 | AnalogInput 4 | Button 5 | Sweep 6 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/BoardConnector.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Internal; 2 | connector BoardConnector = output Integer annotation (Icon(graphics={Ellipse( 3 | extent={{-100,100},{100,-100}}, 4 | fillColor={0,0,0}, 5 | fillPattern=FillPattern.Solid, 6 | pattern=LinePattern.None)})); 7 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/ExternalFirmata.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Internal; 2 | class ExternalFirmata 3 | extends ExternalObject; 4 | 5 | function constructor 6 | import Arduino; 7 | input String port; 8 | input Boolean showCapabilities; 9 | input Integer samplingMs; 10 | input Integer baudRate; 11 | input Boolean useDTR; 12 | input Arduino.Internal.ModelicaFunctions callbacks; 13 | output ExternalFirmata externalFirmata; 14 | external"C" externalFirmata = createFirmataConnection(port, showCapabilities, samplingMs, baudRate, useDTR, callbacks) annotation ( 15 | Library={"ModelicaFirmata"}, 16 | LibraryDirectory="modelica://Arduino/Resources/Library"); 17 | end constructor; 18 | 19 | function destructor 20 | input ExternalFirmata externalFirmata; 21 | external"C" freeFirmataConnection(externalFirmata) annotation ( 22 | Library={"ModelicaFirmata"}, 23 | LibraryDirectory="modelica://Arduino/Resources/Library"); 24 | end destructor; 25 | 26 | annotation(Icon(coordinateSystem(preserveAspectRatio=false, extent={{-100,-100},{100,100}}), graphics={ 27 | Rectangle( 28 | lineColor={160,160,164}, 29 | fillColor={160,160,164}, 30 | fillPattern=FillPattern.Solid, 31 | extent={{-100.0,-100.0},{100.0,100.0}}, 32 | radius=25.0)})); 33 | end ExternalFirmata; 34 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/PinConnector.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Internal; 2 | connector PinConnector = input Integer annotation (Icon(graphics={Ellipse( 3 | extent={{-100,100},{100,-100}}, 4 | fillColor={0,0,0}, 5 | fillPattern=FillPattern.Solid, 6 | pattern=LinePattern.None)})); 7 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/getBoardId.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Internal; 2 | function getBoardId 3 | extends Modelica.Icons.Function; 4 | 5 | input Firmata.Internal.ExternalFirmata board; 6 | output Integer id; 7 | 8 | external "C" id = getBoardId(board) annotation ( 9 | Library={"ModelicaFirmata"}, 10 | LibraryDirectory="modelica://Arduino/Resources/Library"); 11 | end getBoardId; 12 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/numReceivedMessages.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Internal; 2 | function numReceivedMessages 3 | extends Modelica.Icons.Function; 4 | 5 | input Integer id; 6 | output Integer count; 7 | 8 | external "C" count = numReceivedMessages(id) annotation ( 9 | Library={"ModelicaFirmata"}, 10 | LibraryDirectory="modelica://Arduino/Resources/Library"); 11 | end numReceivedMessages; 12 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/numSentMessages.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Internal; 2 | function numSentMessages 3 | extends Modelica.Icons.Function; 4 | 5 | input Integer id; 6 | output Integer count; 7 | 8 | external "C" count = numSentMessages(id) annotation ( 9 | Library={"ModelicaFirmata"}, 10 | LibraryDirectory="modelica://Arduino/Resources/Library"); 11 | end numSentMessages; 12 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/package.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata; 2 | package Internal 3 | extends Modelica.Icons.InternalPackage; 4 | 5 | end Internal; 6 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/package.order: -------------------------------------------------------------------------------- 1 | ExternalFirmata 2 | PinConnector 3 | BoardConnector 4 | readAnalogPin 5 | writeAnalogPin 6 | readDigitalPin 7 | writeDigitalPin 8 | updateBoard 9 | getBoardId 10 | writeServoPin 11 | numSentMessages 12 | numReceivedMessages 13 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/readAnalogPin.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Internal; 2 | function readAnalogPin 3 | extends Modelica.Icons.Function; 4 | 5 | input Integer pin; 6 | input Real min; 7 | input Real max; 8 | input Real init; 9 | input Integer board; 10 | output Real value; 11 | 12 | external "C" value = readAnalogPin(pin, min, max, init, board) annotation ( 13 | Library={"ModelicaFirmata"}, 14 | LibraryDirectory="modelica://Arduino/Resources/Library"); 15 | end readAnalogPin; 16 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/readDigitalPin.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Internal; 2 | function readDigitalPin 3 | extends Modelica.Icons.Function; 4 | 5 | input Integer pin; 6 | input Boolean init; 7 | input Integer board; 8 | input Integer t; 9 | output Boolean value; 10 | 11 | external "C" value = readDigitalPin(pin, init, board, t) annotation ( 12 | Library={"ModelicaFirmata"}, 13 | LibraryDirectory="modelica://Arduino/Resources/Library"); 14 | end readDigitalPin; 15 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/updateBoard.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Internal; 2 | function updateBoard 3 | extends Modelica.Icons.Function; 4 | 5 | input Integer board; 6 | 7 | external "C" updateBoard(board) annotation ( 8 | Library={"ModelicaFirmata"}, 9 | LibraryDirectory="modelica://Arduino/Resources/Library"); 10 | 11 | end updateBoard; 12 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/writeAnalogPin.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Internal; 2 | function writeAnalogPin 3 | extends Modelica.Icons.Function; 4 | 5 | input Integer pin; 6 | input Integer board; 7 | input Real value; 8 | 9 | external "C" writeAnalogPin(pin, board, value) annotation ( 10 | Library={"ModelicaFirmata"}, 11 | LibraryDirectory="modelica://Arduino/Resources/Library"); 12 | end writeAnalogPin; 13 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/writeDigitalPin.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Internal; 2 | function writeDigitalPin 3 | extends Modelica.Icons.Function; 4 | 5 | input Integer pin; 6 | input Integer board; 7 | input Boolean value; 8 | 9 | external "C" writeDigitalPin(pin, board, value) annotation ( 10 | Library={"ModelicaFirmata"}, 11 | LibraryDirectory="modelica://Arduino/Resources/Library"); 12 | end writeDigitalPin; 13 | -------------------------------------------------------------------------------- /Arduino/Firmata/Internal/writeServoPin.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata.Internal; 2 | function writeServoPin 3 | extends Modelica.Icons.Function; 4 | 5 | input Integer pin; 6 | input Integer board; 7 | input Real value; 8 | input Integer minPulse; 9 | input Integer maxPulse; 10 | 11 | external "C" writeServoPin(pin, board, value, minPulse, maxPulse) annotation ( 12 | Library={"ModelicaFirmata"}, 13 | LibraryDirectory="modelica://Arduino/Resources/Library"); 14 | end writeServoPin; 15 | -------------------------------------------------------------------------------- /Arduino/Firmata/PWMOutput.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata; 2 | model PWMOutput 3 | 4 | parameter Integer digitalPin(min=0, max=13) "Digital PWM pin (~)"; 5 | parameter Real minValue = 0 "Minimum value"; 6 | parameter Real maxValue = 1 "Maximum value"; 7 | 8 | Internal.PinConnector pinConnector 9 | annotation (Placement(transformation(extent={{90,-10},{110,10}}), 10 | iconTransformation(extent={{84,-16},{116,16}}))); 11 | Modelica.Blocks.Interfaces.RealInput u 12 | annotation (Placement(transformation(extent={{-140,-20},{-100,20}}))); 13 | 14 | protected 15 | Real scaledValue = (u - minValue) / (maxValue - minValue); 16 | 17 | equation 18 | Firmata.Internal.writeAnalogPin( 19 | digitalPin, 20 | pinConnector, 21 | scaledValue); 22 | 23 | annotation (Icon(coordinateSystem(preserveAspectRatio=false), graphics={Polygon( 24 | points={{100,0},{20,80},{-100,80},{-100,-80},{20,-80},{100,0}}, 25 | lineColor={0,0,0}, 26 | fillColor={255,255,255}, 27 | fillPattern=FillPattern.Solid), Line(points={{-78,-16},{-66,-16},{-66, 28 | 28},{-54,28},{-54,-16},{-14,-16},{-14,28},{28,28},{28,-16},{56, 29 | -16}}, color={28,108,200}), 30 | Text( 31 | extent={{-90,-34},{20,-68}}, 32 | lineColor={0,0,0}, 33 | textString="D%digitalPin")}), Diagram(coordinateSystem( 34 | preserveAspectRatio=false))); 35 | end PWMOutput; 36 | -------------------------------------------------------------------------------- /Arduino/Firmata/Port.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata; 2 | model Port 3 | 4 | Internal.BoardConnector boardConnector 5 | annotation (Placement(transformation(extent={{-110,-10},{-90,10}}), 6 | iconTransformation(extent={{-116,-16},{-84,16}}))); 7 | parameter String port "Serial port (empty = auto)"; 8 | parameter Modelica.Units.SI.Time sampleInterval( 9 | displayUnit="ms", 10 | min=0.001, 11 | max=0.1) = 0.001 "Sample interval"; 12 | Firmata.Internal.ExternalFirmata externalFirmata= 13 | Firmata.Internal.ExternalFirmata( 14 | port, 15 | false, 16 | integer(sampleInterval*1000), 17 | baudRate, 18 | false, 19 | Arduino.Internal.ModelicaFunctions()); 20 | Integer numSentMessages; 21 | Integer numReceivedMessages; 22 | 23 | protected 24 | constant Integer baudRate = 57600; 25 | 26 | Integer boardId(start = -1); 27 | 28 | public 29 | Modelica_DeviceDrivers.Blocks.OperatingSystem.RealtimeSynchronize 30 | realtimeSynchronize 31 | annotation (Placement(transformation(extent={{-10,-10},{10,10}}))); 32 | equation 33 | when initial() then 34 | boardId = Firmata.Internal.getBoardId(externalFirmata); 35 | end when; 36 | 37 | when sample(0, sampleInterval) then 38 | Firmata.Internal.updateBoard(boardId); 39 | numSentMessages = Arduino.Firmata.Internal.numSentMessages(boardId); 40 | numReceivedMessages = Arduino.Firmata.Internal.numReceivedMessages(boardId); 41 | end when; 42 | 43 | boardConnector = boardId; 44 | 45 | annotation (Icon(coordinateSystem(preserveAspectRatio=false), graphics={ 46 | Rectangle(extent={{40,40},{100,-40}}, lineColor={34,34,34}, 47 | fillColor={215,215,215}, 48 | fillPattern=FillPattern.Solid), 49 | Rectangle( 50 | extent={{60,-10},{80,-20}}, 51 | fillColor={34,34,34}, 52 | fillPattern=FillPattern.Solid, 53 | pattern=LinePattern.None, 54 | lineColor={0,0,0}), 55 | Rectangle( 56 | extent={{-100,6},{-40,-6}}, 57 | fillColor={0,0,0}, 58 | fillPattern=FillPattern.Solid, 59 | pattern=LinePattern.None, 60 | lineColor={0,0,0}), 61 | Rectangle( 62 | fillColor={34,34,34}, 63 | extent={{-60,-60},{40,60}}, 64 | radius=7, 65 | fillPattern=FillPattern.Solid, 66 | pattern=LinePattern.None, 67 | lineColor={0,0,0}), 68 | Rectangle( 69 | extent={{60,20},{80,10}}, 70 | fillColor={34,34,34}, 71 | fillPattern=FillPattern.Solid, 72 | pattern=LinePattern.None, 73 | lineColor={0,0,0}), 74 | Text( 75 | extent={{-54,-16},{32,18}}, 76 | lineColor={255,255,255}, 77 | textString="%port")}), Diagram(coordinateSystem( 78 | preserveAspectRatio=false))); 79 | end Port; 80 | -------------------------------------------------------------------------------- /Arduino/Firmata/Servo.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Firmata; 2 | model Servo 3 | 4 | parameter Integer digitalPin(min=0, max=13) "Digital PWM pin (~)"; 5 | parameter Integer minPulse = 544 "Minimum pulse"; 6 | parameter Integer maxPulse = 2400 "Maximum pulse"; 7 | 8 | Internal.PinConnector pinConnector 9 | annotation (Placement(transformation(extent={{90,-10},{110,10}}), 10 | iconTransformation(extent={{84,-16},{116,16}}))); 11 | Modelica.Blocks.Interfaces.RealInput u 12 | annotation (Placement(transformation(extent={{-140,-20},{-100,20}}))); 13 | 14 | equation 15 | Firmata.Internal.writeServoPin( 16 | digitalPin, 17 | pinConnector, 18 | u, 19 | minPulse, 20 | maxPulse); 21 | 22 | annotation (Icon(coordinateSystem(preserveAspectRatio=false), graphics={ 23 | Polygon( 24 | points={{100,0},{20,80},{-100,80},{-100,-80},{20,-80},{100,0}}, 25 | lineColor={0,0,0}, 26 | fillColor={255,255,255}, 27 | fillPattern=FillPattern.Solid), 28 | Rectangle( 29 | extent={{-38,44},{14,-50}}, 30 | lineColor={0,0,0}, 31 | fillColor={255,255,255}, 32 | fillPattern=FillPattern.Solid), 33 | Polygon( 34 | points={{-8,18},{36,28},{44,12},{8,-18},{-8,18}}, 35 | lineColor={0,0,0}, 36 | fillColor={255,255,255}, 37 | fillPattern=FillPattern.Solid), 38 | Ellipse( 39 | extent={{-20,20},{20,-20}}, 40 | lineColor={0,0,0}, 41 | fillColor={255,255,255}, 42 | fillPattern=FillPattern.Solid), 43 | Ellipse( 44 | extent={{30,30},{50,10}}, 45 | lineColor={0,0,0}, 46 | fillColor={255,255,255}, 47 | fillPattern=FillPattern.Solid), 48 | Polygon( 49 | points={{-4,18},{40,28},{46,14},{8,-18},{-4,18}}, 50 | fillColor={255,255,255}, 51 | fillPattern=FillPattern.Solid, 52 | pattern=LinePattern.None), 53 | Ellipse( 54 | extent={{-8,8},{8,-8}}, 55 | lineColor={0,0,0}, 56 | fillColor={255,255,255}, 57 | fillPattern=FillPattern.Solid), 58 | Ellipse( 59 | extent={{36,24},{44,16}}, 60 | lineColor={0,0,0}, 61 | fillColor={255,255,255}, 62 | fillPattern=FillPattern.Solid), 63 | Polygon( 64 | points={{-12,60},{0,68},{12,66},{-12,60}}, 65 | pattern=LinePattern.None, 66 | fillColor={255,255,255}, 67 | fillPattern=FillPattern.Solid), 68 | Text( 69 | extent={{-94,18},{-44,-18}}, 70 | lineColor={0,0,0}, 71 | textString="D%digitalPin")}), Diagram(coordinateSystem( 72 | preserveAspectRatio=false))); 73 | end Servo; 74 | -------------------------------------------------------------------------------- /Arduino/Firmata/package.mo: -------------------------------------------------------------------------------- 1 | within Arduino; 2 | package Firmata "Library to connect models to real-world circuits using the Firmata protocol" 3 | extends Modelica.Icons.Package; 4 | 5 | annotation ( Icon(graphics={ 6 | Rectangle(extent={{20,38},{76,-40}}, lineColor={80,80,80}, 7 | fillColor={215,215,215}, 8 | fillPattern=FillPattern.Solid), 9 | Rectangle( 10 | extent={{36,-12},{56,-20}}, 11 | fillColor={80,80,80}, 12 | fillPattern=FillPattern.Solid, 13 | pattern=LinePattern.None, 14 | lineColor={0,0,0}), 15 | Rectangle( 16 | extent={{-84,6},{-56,-6}}, 17 | fillColor={50,50,50}, 18 | fillPattern=FillPattern.Solid, 19 | pattern=LinePattern.None, 20 | lineColor={0,0,0}), 21 | Rectangle( 22 | fillColor={80,80,80}, 23 | extent={{-56,-52},{20,52}}, 24 | radius=7, 25 | fillPattern=FillPattern.Solid, 26 | pattern=LinePattern.None, 27 | lineColor={0,0,0}), 28 | Rectangle( 29 | extent={{36,18},{56,10}}, 30 | fillColor={80,80,80}, 31 | fillPattern=FillPattern.Solid, 32 | pattern=LinePattern.None, 33 | lineColor={0,0,0})})); 34 | end Firmata; 35 | -------------------------------------------------------------------------------- /Arduino/Firmata/package.order: -------------------------------------------------------------------------------- 1 | Examples 2 | Port 3 | AnalogInput 4 | PWMOutput 5 | DigitalInput 6 | DigitalOutput 7 | Servo 8 | Internal 9 | -------------------------------------------------------------------------------- /Arduino/Internal/ExternalArduino.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Internal; 2 | class ExternalArduino "External object of Arduino" 3 | extends ExternalObject; 4 | 5 | function constructor "Open Arduino" 6 | input Arduino.Internal.ModelicaFunctions callbacks; 7 | output ExternalArduino externalArduino; 8 | external"C" externalArduino = 9 | ModelicaArduino_open(callbacks) annotation ( 10 | Library="ModelicaArduino"); 11 | end constructor; 12 | 13 | function destructor "Close Arduino" 14 | input ExternalArduino externalArduino; 15 | external"C" ModelicaArduino_close(externalArduino) annotation ( 16 | Library="ModelicaArduino"); 17 | end destructor; 18 | 19 | annotation(Icon(coordinateSystem(preserveAspectRatio=false, extent={{-100,-100},{100,100}}), graphics={ 20 | Rectangle( 21 | lineColor={160,160,164}, 22 | fillColor={160,160,164}, 23 | fillPattern=FillPattern.Solid, 24 | extent={{-100.0,-100.0},{100.0,100.0}}, 25 | radius=25.0)})); 26 | end ExternalArduino; 27 | -------------------------------------------------------------------------------- /Arduino/Internal/ModelicaFunctions.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Internal; 2 | class ModelicaFunctions 3 | extends ExternalObject; 4 | 5 | function constructor "Function that call the external constructor" 6 | output ModelicaFunctions functions; 7 | 8 | external "C" functions = ModelicaUtilityFunctions_getModelicaUtilityFunctions() annotation ( 9 | Include = "#include \"ModelicaUtilityFunctions.c\"", 10 | IncludeDirectory="modelica://Arduino/Resources/Include"); 11 | end constructor; 12 | 13 | function destructor 14 | input ModelicaFunctions functions; 15 | external "C" ModelicaUtilityFunctions_freeModelicaUtilityFunctions(functions) annotation ( 16 | Include = "#include \"ModelicaUtilityFunctions.c\"", 17 | IncludeDirectory="modelica://Arduino/Resources/Include"); 18 | end destructor; 19 | 20 | annotation(Icon(coordinateSystem(preserveAspectRatio=false, extent={{-100,-100},{100,100}}), graphics={ 21 | Rectangle( 22 | lineColor={160,160,164}, 23 | fillColor={160,160,164}, 24 | fillPattern=FillPattern.Solid, 25 | extent={{-100.0,-100.0},{100.0,100.0}}, 26 | radius=25.0)})); 27 | end ModelicaFunctions; 28 | -------------------------------------------------------------------------------- /Arduino/Internal/PinDriver.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Internal; 2 | model PinDriver 3 | 4 | Modelica.Electrical.Analog.Interfaces.PositivePin 5 | y 6 | annotation (Placement(transformation(extent={{90,-10},{110,10}}))); 7 | Modelica.Electrical.Analog.Ideal.IdealOpeningSwitch idealOpeningSwitch(Goff= 8 | 1e-9) 9 | annotation (Placement(transformation(extent={{-10,10},{10,-10}}, 10 | rotation=180, 11 | origin={70,0}))); 12 | Modelica.Electrical.Analog.Ideal.IdealTwoWaySwitch idealCommutingSwitch 13 | annotation (Placement(transformation(extent={{40,10},{20,-10}}))); 14 | VariableBooleanPulse variableBooleanPulse(sampleInterval=sampleInterval) 15 | annotation (Placement(transformation(extent={{-40,-30},{-20,-10}}))); 16 | Modelica.Blocks.Interfaces.IntegerInput pulseWidth 17 | "Pulse width in milliseconds" 18 | annotation (Placement(transformation(extent={{-140,-80},{-100,-40}}))); 19 | Modelica.Blocks.Interfaces.IntegerInput pulsePeriod 20 | "Pulse time in milliseconds" 21 | annotation (Placement(transformation(extent={{-140,-20},{-100,20}}))); 22 | Modelica.Electrical.Analog.Interfaces.NegativePin 23 | ground 24 | annotation (Placement(transformation(extent={{90,-70},{110,-50}}))); 25 | Modelica.Electrical.Analog.Interfaces.PositivePin v_in 26 | "Positive pin (potential p.v > n.v for positive voltage drop v)" 27 | annotation (Placement(transformation(extent={{90,50},{110,70}}))); 28 | parameter Modelica.Units.SI.Time sampleInterval=1e-2; 29 | Modelica.Blocks.Sources.BooleanExpression booleanExpression(y=portMode == 2) 30 | annotation (Placement(transformation(extent={{-70,-24},{-50,-4}}))); 31 | Modelica.Blocks.Interfaces.IntegerInput portMode "Port mode" 32 | annotation (Placement(transformation(extent={{-140,40},{-100,80}}))); 33 | Modelica.Blocks.Sources.BooleanExpression booleanExpression1(y=portMode == 0) 34 | annotation (Placement(transformation(extent={{40,20},{60,40}}))); 35 | equation 36 | connect(y, idealOpeningSwitch.p) 37 | annotation (Line(points={{100,0},{80,0}}, color={0,0,255})); 38 | connect(idealCommutingSwitch.p, idealOpeningSwitch.n) 39 | annotation (Line(points={{40,0},{60,0}}, color={0,0,255})); 40 | connect(variableBooleanPulse.width, pulseWidth) annotation (Line(points={{-42,-26}, 41 | {-80,-26},{-80,-60},{-120,-60}}, color={255,127,0})); 42 | connect(variableBooleanPulse.period, pulsePeriod) annotation (Line(points={{-42,-20}, 43 | {-80,-20},{-80,0},{-120,0}}, color={255,127,0})); 44 | connect(idealCommutingSwitch.n2, v_in) 45 | annotation (Line(points={{20,0},{0,0},{0,60},{100,60}}, color={0,0,255})); 46 | connect(ground, idealCommutingSwitch.n1) annotation (Line(points={{100,-60},{ 47 | 0,-60},{0,-4},{20,-4}}, color={0,0,255})); 48 | connect(variableBooleanPulse.y, idealCommutingSwitch.control) 49 | annotation (Line(points={{-19,-20},{30,-20},{30,-12}},color={255,0,255})); 50 | connect(booleanExpression.y, variableBooleanPulse.pwm) 51 | annotation (Line(points={{-49,-14},{-42,-14}}, color={255,0,255})); 52 | connect(booleanExpression1.y, idealOpeningSwitch.control) 53 | annotation (Line(points={{61,30},{70,30},{70,12}},color={255,0,255})); 54 | annotation (Icon(coordinateSystem(preserveAspectRatio=false), graphics={ 55 | Rectangle( 56 | extent={{-100,100},{100,-100}}, 57 | fillColor={210,210,210}, 58 | lineThickness=5.0, 59 | fillPattern=FillPattern.Solid, 60 | borderPattern=BorderPattern.Raised)}), Diagram( 61 | coordinateSystem(preserveAspectRatio=false))); 62 | end PinDriver; 63 | -------------------------------------------------------------------------------- /Arduino/Internal/VariableBooleanPulse.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Internal; 2 | block VariableBooleanPulse "Generate pulse signal of type Boolean" 3 | extends Modelica.Blocks.Interfaces.partialBooleanSource; 4 | 5 | parameter Modelica.Units.SI.Time startTime=0 "Time instant of first pulse"; 6 | 7 | parameter Modelica.Units.SI.Time sampleInterval(fixed=true) = 1e-2; 8 | 9 | public 10 | Modelica.Blocks.Interfaces.BooleanInput pwm(start=false) annotation (Placement(transformation(extent={{-140,40},{-100,80}}))); 11 | Modelica.Blocks.Interfaces.IntegerInput period(start=2000, fixed=true) 12 | "Pulse time in microseconds" 13 | annotation (Placement(transformation(extent={{-140,-20},{-100,20}}))); 14 | Modelica.Blocks.Interfaces.IntegerInput width(start=100, fixed=true) "Pulse width in microseconds" 15 | annotation (Placement(transformation(extent={{-140,-80},{-100,-40}}))); 16 | 17 | protected 18 | discrete Modelica.Units.SI.Time nextTimeEvent(start=0, fixed=true) 19 | "Next time event instant"; 20 | 21 | equation 22 | 23 | when {time >= pre(nextTimeEvent),initial()} then 24 | 25 | if pwm then 26 | 27 | if width == 0 then 28 | nextTimeEvent = time + period * 1e-6; 29 | elseif mod(time * 1e6, period) < width then 30 | nextTimeEvent = time + (width) * 1e-6; 31 | else 32 | nextTimeEvent = time + (period-width) * 1e-6; 33 | end if; 34 | 35 | else 36 | nextTimeEvent = time + sampleInterval; 37 | end if; 38 | 39 | //nextTimeEvent = time + 100 * 1e-6; 40 | //Modelica.Utilities.Streams.print(String(pre(time)) + " -> " + String(time)); 41 | //nextTimeEvent = time + 100 * 1e-6; 42 | y = mod(time * 1e6 + 1, period) < width; 43 | end when; 44 | 45 | annotation ( 46 | Icon(coordinateSystem( 47 | preserveAspectRatio=true, 48 | extent={{-100,-100},{100,100}}), graphics={Text( 49 | extent={{-150,-140},{150,-110}}, 50 | lineColor={0,0,0}, 51 | textString="%period"), Line(points={{-80,-70},{-40,-70},{-40,44},{0, 52 | 44},{0,-70},{40,-70},{40,44},{79,44}})}), 53 | Diagram(coordinateSystem( 54 | preserveAspectRatio=true, 55 | extent={{-100,-100},{100,100}}), graphics={ 56 | Text( 57 | extent={{-60,-74},{-19,-82}}, 58 | lineColor={0,0,0}, 59 | textString="startTime"), 60 | Line( 61 | points={{-78,-70},{-40,-70},{-40,20},{20,20},{20,-70},{50,-70},{50, 62 | 20},{100,20}}, 63 | color={0,0,255}, 64 | thickness=0.5), 65 | Line(points={{-40,61},{-40,21}}, color={95,95,95}), 66 | Line(points={{20,44},{20,20}}, color={95,95,95}), 67 | Line(points={{50,58},{50,20}}, color={95,95,95}), 68 | Line(points={{-40,53},{50,53}}, color={95,95,95}), 69 | Line(points={{-40,35},{20,35}}, color={95,95,95}), 70 | Text( 71 | extent={{-30,65},{16,55}}, 72 | lineColor={0,0,0}, 73 | textString="period"), 74 | Text( 75 | extent={{-33,47},{14,37}}, 76 | lineColor={0,0,0}, 77 | textString="width"), 78 | Line(points={{-70,20},{-41,20}}, color={95,95,95}), 79 | Polygon( 80 | points={{-40,35},{-31,37},{-31,33},{-40,35}}, 81 | lineColor={95,95,95}, 82 | fillColor={95,95,95}, 83 | fillPattern=FillPattern.Solid), 84 | Polygon( 85 | points={{20,35},{12,37},{12,33},{20,35}}, 86 | lineColor={95,95,95}, 87 | fillColor={95,95,95}, 88 | fillPattern=FillPattern.Solid), 89 | Polygon( 90 | points={{-40,53},{-31,55},{-31,51},{-40,53}}, 91 | lineColor={95,95,95}, 92 | fillColor={95,95,95}, 93 | fillPattern=FillPattern.Solid), 94 | Polygon( 95 | points={{50,53},{42,55},{42,51},{50,53}}, 96 | lineColor={95,95,95}, 97 | fillColor={95,95,95}, 98 | fillPattern=FillPattern.Solid), 99 | Text( 100 | extent={{-95,26},{-66,17}}, 101 | lineColor={0,0,0}, 102 | textString="true"), 103 | Text( 104 | extent={{-96,-60},{-75,-69}}, 105 | lineColor={0,0,0}, 106 | textString="false")}), 107 | Documentation(info=" 108 |

109 | The Boolean output y is a pulse signal: 110 |

111 | 112 |

113 | \"Pulse.png\" 115 |

116 | ")); 117 | end VariableBooleanPulse; 118 | -------------------------------------------------------------------------------- /Arduino/Internal/WavRecorder.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Internal; 2 | class WavRecorder 3 | extends ExternalObject; 4 | 5 | function constructor 6 | input String filename; 7 | output WavRecorder wavRecorder; 8 | 9 | external "C" wavRecorder = WavRecorder_create(filename) annotation ( 10 | Include = "#include \"WavRecorder.c\"", 11 | IncludeDirectory="modelica://Arduino/Resources/Include"); 12 | end constructor; 13 | 14 | function destructor 15 | input WavRecorder wavRecorder; 16 | external "C" WavRecorder_free(wavRecorder) annotation ( 17 | Include = "#include \"WavRecorder.c\"", 18 | IncludeDirectory="modelica://Arduino/Resources/Include"); 19 | end destructor; 20 | 21 | annotation(Icon(coordinateSystem(preserveAspectRatio=false, extent={{-100,-100},{100,100}}), graphics={ 22 | Rectangle( 23 | lineColor={160,160,164}, 24 | fillColor={160,160,164}, 25 | fillPattern=FillPattern.Solid, 26 | extent={{-100.0,-100.0},{100.0,100.0}}, 27 | radius=25.0)})); 28 | end WavRecorder; 29 | -------------------------------------------------------------------------------- /Arduino/Internal/buildSketch.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Internal; 2 | encapsulated function buildSketch 3 | extends Modelica.Icons.Function; 4 | 5 | import Arduino; 6 | import Modelica; 7 | input String sketch = "Blink.ino"; 8 | input String cmake = "cmake"; 9 | input String generator = "Visual Studio 17 2022"; 10 | input String platform = "Win32"; 11 | output Boolean success; 12 | 13 | protected 14 | String path = Modelica.Utilities.Files.loadResource("modelica://Arduino/"); 15 | String cppFile = path + "Resources/Source/Arduino/Sketch.cpp"; 16 | String batFile = "buildSketch.bat"; 17 | 18 | algorithm 19 | 20 | // remove the .cpp file 21 | Modelica.Utilities.Files.removeFile(cppFile); 22 | 23 | // write the C++ file 24 | Modelica.Utilities.Streams.print("#include \"Sketch.h\" 25 | 26 | #include \"Arduino.h\" 27 | #include \"SoftSerial.h\" 28 | 29 | SoftSerial Serial; 30 | 31 | // include your sketch here 32 | #include \"" + sketch + "\"", cppFile); 33 | 34 | // remove the build script 35 | Modelica.Utilities.Files.removeFile(batFile); 36 | 37 | // write the build script 38 | Modelica.Utilities.Streams.print( 39 | "set CMAKE=\"" + cmake + "\"\n" + 40 | "set BUILD=\"" + path + "Resources/Source/Arduino/" + platform + "\"\n" + 41 | "\n" + 42 | "%CMAKE% -G \"" + generator + "\" -A " + platform + " -S \"" + path + "Resources/Source/Arduino\" -B %BUILD%\n" + 43 | "if %errorlevel% neq 0 exit /b %errorlevel%\n" + 44 | "\n" + 45 | "%CMAKE% --build %BUILD% --config Release\n" + 46 | "if %errorlevel% neq 0 exit /b %errorlevel%\n", batFile); 47 | 48 | // call the build script 49 | success := Modelica.Utilities.System.command("CALL " + batFile) == 0; 50 | 51 | annotation(__Dymola_translate=true); 52 | end buildSketch; 53 | -------------------------------------------------------------------------------- /Arduino/Internal/buildSketchOM.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Internal; 2 | encapsulated function buildSketchOM 3 | extends Modelica.Icons.Function; 4 | 5 | import Arduino; 6 | import Modelica; 7 | input String sketch = "Blink.ino"; 8 | input String cmake = "C:/Program Files/CMake/bin/cmake.exe" "Absolute path to the CMake executable"; 9 | input String generator = "Visual Studio 17 2022" "CMake generator to build the Sketch" annotation(choices(choice="Visual Studio 14 2015", choice="Visual Studio 15 2017", choice="Visual Studio 16 2019", choice="Visual Studio 17 2022")); 10 | input String platform = "x64" annotation(choices(choice="Win32", choice="x64")); 11 | output Boolean success; 12 | 13 | protected 14 | String path = Modelica.Utilities.Files.loadResource("modelica://Arduino/"); 15 | String cppFile = path + "Resources/Source/Arduino/Sketch.cpp"; 16 | String batFile = "buildSketch.bat"; 17 | 18 | algorithm 19 | 20 | // remove the .cpp file 21 | remove(cppFile); 22 | 23 | // write the C++ file 24 | writeFile(cppFile, "#include \"Sketch.h\" 25 | 26 | #include \"Arduino.h\" 27 | #include \"SoftSerial.h\" 28 | 29 | SoftSerial Serial; 30 | 31 | // include your sketch here 32 | #include \"" + sketch + "\""); 33 | 34 | // remove the build script 35 | remove(batFile); 36 | 37 | // write the build script 38 | writeFile(batFile, 39 | "set CMAKE=\"" + cmake + "\"\n" + 40 | "set BUILD=\"" + path + "Resources/Source/Arduino/" + platform + "\"\n" + 41 | "\n" + 42 | "%CMAKE% -G \"" + generator + "\" -A " + platform + " -S \"" + path + "Resources/Source/Arduino\" -B %BUILD%\n" + 43 | "if %errorlevel% neq 0 exit /b %errorlevel%\n" + 44 | "\n" + 45 | "%CMAKE% --build %BUILD% --config Release\n" + 46 | "if %errorlevel% neq 0 exit /b %errorlevel%\n"); 47 | 48 | // call the build script 49 | success := system(batFile + " > buildSketch.log") == 0; 50 | 51 | end buildSketchOM; 52 | -------------------------------------------------------------------------------- /Arduino/Internal/clean.mo: -------------------------------------------------------------------------------- 1 | within Arduino.Internal; 2 | function clean "Remove the generated CMake project" 3 | extends Modelica.Icons.Function; 4 | 5 | output Boolean success; 6 | 7 | protected 8 | String path = Modelica.Utilities.Files.loadResource("modelica://Arduino/"); 9 | 10 | algorithm 11 | success := system("rmdir \"" + path + "Resources/Source/Arduino/x64\" /s /q") == 0; 12 | 13 | end clean; 14 | -------------------------------------------------------------------------------- /Arduino/Internal/package.mo: -------------------------------------------------------------------------------- 1 | within Arduino; 2 | package Internal 3 | extends Modelica.Icons.InternalPackage; 4 | 5 | 6 | 7 | 8 | end Internal; 9 | -------------------------------------------------------------------------------- /Arduino/Internal/package.order: -------------------------------------------------------------------------------- 1 | ModelicaFunctions 2 | ExternalArduino 3 | WavRecorder 4 | PinDriver 5 | VariableBooleanPulse 6 | buildSketch 7 | buildSketchOM 8 | clean 9 | -------------------------------------------------------------------------------- /Arduino/Resources/Images/AnalogRead_bb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Images/AnalogRead_bb.png -------------------------------------------------------------------------------- /Arduino/Resources/Images/Blink_bb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Images/Blink_bb.png -------------------------------------------------------------------------------- /Arduino/Resources/Images/Button_bb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Images/Button_bb.png -------------------------------------------------------------------------------- /Arduino/Resources/Images/Fade_bb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Images/Fade_bb.png -------------------------------------------------------------------------------- /Arduino/Resources/Images/Servo_bb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Images/Servo_bb.png -------------------------------------------------------------------------------- /Arduino/Resources/Images/blink_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Images/blink_example.png -------------------------------------------------------------------------------- /Arduino/Resources/Images/blink_example_om.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Images/blink_example_om.png -------------------------------------------------------------------------------- /Arduino/Resources/Images/simple.css: -------------------------------------------------------------------------------- 1 | body { 2 | margin: 20px 20px 20px 30px; 3 | font-family: -apple-system, BlinkMacSystemFont, "Segoe UI", Roboto, Oxygen-Sans, Ubuntu, Cantarell, "Helvetica Neue", sans-serif; 4 | font-size: 14px; 5 | } 6 | 7 | p code, li code { 8 | padding: 2px 4px; 9 | font-size: 90%; 10 | color: #c7254e; 11 | background-color: #f9f2f4; 12 | border-radius: 3px; 13 | } 14 | 15 | p { 16 | margin: 6px 0 0; 17 | } 18 | 19 | h1, h2, h3, h4, h5 { 20 | margin: 16px 0 10px; 21 | } 22 | 23 | a { 24 | color: #3777b0; 25 | text-decoration: none; 26 | } 27 | 28 | ol, ul { 29 | padding: 0; 30 | margin: 3px 0 3px 28px !important; 31 | } 32 | 33 | li { 34 | line-height: 1.6em; 35 | display: list-item; 36 | } 37 | 38 | li p { 39 | margin: 0; 40 | } 41 | 42 | .codehilite { 43 | background-color: #fff; 44 | color: #333; 45 | border: 1px solid rgb(229, 229, 229); 46 | padding: 9.5px; 47 | } 48 | 49 | .codehilite pre { 50 | margin: 0; 51 | font-size: 13px; 52 | line-height: 1.6em; 53 | overflow-x: auto; 54 | border-radius: 2px; 55 | } 56 | 57 | .highlight { 58 | text-shadow: none; 59 | } 60 | 61 | table { 62 | width: 100%; 63 | border-collapse: collapse; 64 | overflow-x: scroll; 65 | } 66 | 67 | table { 68 | display: table; 69 | border-collapse: separate; 70 | border-spacing: 2px; 71 | border-color: grey; 72 | } 73 | 74 | table td, table th { 75 | text-align: left; 76 | padding: 5px 10px; 77 | border-bottom: 1px solid #e5e5e5; 78 | } 79 | 80 | hr { 81 | border: 0; 82 | border-top: 1px solid #e5e5e5; 83 | margin: 1em 0 1em 0; 84 | } 85 | -------------------------------------------------------------------------------- /Arduino/Resources/Include/ModelicaArduino.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifdef _MSC_VER 4 | #define EXPORT __declspec(dllexport) 5 | #else 6 | #define EXPORT __attribute__((visibility("default"))) 7 | #endif 8 | 9 | #ifdef __cplusplus 10 | extern "C" { 11 | #endif 12 | 13 | EXPORT void * ModelicaArduino_open(void *callbacks); 14 | 15 | EXPORT void ModelicaArduino_close(void *externalObject); 16 | 17 | EXPORT void ModelicaArduino_update(void *instance, 18 | double time, 19 | double analogReference, 20 | /*in*/ double *potential, 21 | /*out*/ int *portMode, 22 | /*out*/ int *pulseWidth, 23 | /*out*/ int *pulsePeriod); 24 | 25 | #ifdef __cplusplus 26 | } // extern "C" 27 | #endif 28 | -------------------------------------------------------------------------------- /Arduino/Resources/Include/ModelicaUtilityFunctions.c: -------------------------------------------------------------------------------- 1 | #ifndef MODELICA_UTILITY_FUNCTIONS_C 2 | #define MODELICA_UTILITY_FUNCTIONS_C 3 | 4 | #include "ModelicaUtilities.h" 5 | #include "ModelicaUtilityFunctions.h" 6 | 7 | 8 | static ModelicaUtilityFunctions_t s_ModelicaUtilityFunctions; 9 | 10 | 11 | void * ModelicaUtilityFunctions_getModelicaUtilityFunctions() { 12 | 13 | s_ModelicaUtilityFunctions.ModelicaMessage = ModelicaMessage; 14 | s_ModelicaUtilityFunctions.ModelicaFormatMessage = ModelicaFormatMessage; 15 | s_ModelicaUtilityFunctions.ModelicaVFormatMessage = ModelicaVFormatMessage; 16 | s_ModelicaUtilityFunctions.ModelicaError = ModelicaError; 17 | s_ModelicaUtilityFunctions.ModelicaFormatError = ModelicaFormatError; 18 | s_ModelicaUtilityFunctions.ModelicaVFormatError = ModelicaVFormatError; 19 | s_ModelicaUtilityFunctions.ModelicaAllocateString = ModelicaAllocateString; 20 | s_ModelicaUtilityFunctions.ModelicaAllocateStringWithErrorReturn = ModelicaAllocateStringWithErrorReturn; 21 | 22 | return &s_ModelicaUtilityFunctions; 23 | } 24 | 25 | 26 | void ModelicaUtilityFunctions_freeModelicaUtilityFunctions(void *functions) { 27 | // nothing to do 28 | } 29 | 30 | #endif // MODELICA_UTILITY_FUNCTIONS_C 31 | -------------------------------------------------------------------------------- /Arduino/Resources/Include/ModelicaUtilityFunctions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | typedef struct { 5 | 6 | void (*ModelicaMessage)(const char *string); 7 | void (*ModelicaFormatMessage)(const char *string, ...); 8 | void (*ModelicaVFormatMessage)(const char *string, va_list); 9 | void (*ModelicaError)(const char *string); 10 | void (*ModelicaFormatError)(const char *string, ...); 11 | void (*ModelicaVFormatError)(const char *string, va_list); 12 | char* (*ModelicaAllocateString)(size_t len); 13 | char* (*ModelicaAllocateStringWithErrorReturn)(size_t len); 14 | 15 | } ModelicaUtilityFunctions_t; 16 | 17 | 18 | void setModelicaUtilityFunctions(ModelicaUtilityFunctions_t *callbacks); 19 | -------------------------------------------------------------------------------- /Arduino/Resources/Include/WavRecorder.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "ModelicaUtilities.h" 5 | 6 | /* make sure all compiler use the same alignment policies for structures */ 7 | #if defined _MSC_VER || defined __GNUC__ 8 | #pragma pack(push,2) 9 | #endif 10 | 11 | typedef struct { 12 | char formatID[4]; 13 | unsigned long fileSize; 14 | char riffType[4]; 15 | char chunkID[4]; 16 | unsigned long extData; // extension data 17 | unsigned short formatTag; // PCM - integer samples 18 | unsigned short channels; // 1 (mono) or 2 (stereo) 19 | unsigned long samplesPerSec; // samples per second (Hz) 20 | unsigned long bytesPerSec; // (Sample Rate * BitsPerSample * Channels) / 8 21 | unsigned short blockAlign; // data block size (size of two integer samples, one for each channel, in bytes) 22 | unsigned short bitsPerSample; // number of bits per sample (use a multiple of 8) 23 | char dataID[4]; 24 | unsigned long dataSize; 25 | } WavHeader; 26 | 27 | typedef struct { 28 | FILE* file; 29 | long n_samples; 30 | WavHeader header; 31 | } WavFile; 32 | 33 | /* reset alignment policy to the one set before reading this file */ 34 | #if defined _MSC_VER || defined __GNUC__ 35 | #pragma pack(pop) 36 | #endif 37 | 38 | void* WavRecorder_create(const char* filename) { 39 | 40 | WavFile* speaker = (WavFile*)malloc(sizeof(WavFile)); 41 | 42 | if (!speaker) { 43 | ModelicaFormatError("Failed to allocate memory for WAV file recorder."); 44 | return NULL; 45 | } 46 | 47 | speaker->file = fopen(filename, "wb"); // w for write, b for binary 48 | 49 | if (!speaker->file) { 50 | ModelicaFormatError("Failed to open \"%s\" for writing.", filename); 51 | return NULL; 52 | } 53 | 54 | speaker->header = (WavHeader){ 55 | .formatID = "RIFF", 56 | .fileSize = 0, 57 | .riffType = "WAVE", 58 | .chunkID = "fmt ", 59 | .extData = 16, 60 | .formatTag = 1, 61 | .channels = 1, 62 | .samplesPerSec = 44100, 63 | .bytesPerSec = 88200, 64 | .blockAlign = 2, 65 | .bitsPerSample = 16, 66 | .dataID = "data", 67 | .dataSize = 0 68 | }; 69 | 70 | speaker->n_samples = 0; 71 | 72 | fseek(speaker->file, sizeof(WavHeader), SEEK_SET); 73 | 74 | return speaker; 75 | } 76 | 77 | void WavRecorder_free(void* object) { 78 | 79 | WavFile* speaker = (WavFile*)object; 80 | 81 | // (We'll need the final file size to fix the chunk sizes above) 82 | const long file_length = ftell(speaker->file); 83 | 84 | speaker->header.fileSize = file_length - 8; 85 | 86 | // Fix the file header to contain the proper RIFF chunk size, which is (file size - 8) bytes 87 | fseek(speaker->file, 0, SEEK_SET); 88 | 89 | fwrite(&speaker->header, sizeof(WavHeader), 1, speaker->file); 90 | 91 | fclose(speaker->file); 92 | 93 | free(speaker); 94 | } 95 | 96 | void WavRecorder_sample(void* object, double time, double value) { 97 | 98 | WavFile* speaker = (WavFile*)object; 99 | 100 | unsigned short s = value * SHRT_MAX; 101 | 102 | const size_t last_sample = time * speaker->header.samplesPerSec; 103 | 104 | while (speaker->n_samples < last_sample) { 105 | fwrite(&s, sizeof(s), 1, speaker->file); 106 | speaker->n_samples++; 107 | speaker->header.dataSize += sizeof(unsigned short); 108 | } 109 | } 110 | -------------------------------------------------------------------------------- /Arduino/Resources/Library/win64/.gitignore: -------------------------------------------------------------------------------- 1 | # ignore all files in this folder 2 | * 3 | !.gitignore 4 | -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_3DPart028615__2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_3DPart028615__2.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_Case4Servo__10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_Case4Servo__10.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_Gripper_Came_4Servo__31.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_Gripper_Came_4Servo__31.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_Gripper_Connecting_Rod_4Servo__32.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_Gripper_Connecting_Rod_4Servo__32.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_Gripper_Connecting_Rod_4Servo__35.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_Gripper_Connecting_Rod_4Servo__35.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_Gripper_Foam_4Servo__3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_Gripper_Foam_4Servo__3.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_Gripper_Left_Arm_4Servo__33.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_Gripper_Left_Arm_4Servo__33.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_Gripper_Main_4Servo__29.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_Gripper_Main_4Servo__29.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_Gripper_Right_Arm_4Servo__34.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_Gripper_Right_Arm_4Servo__34.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_L4Servo__21.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_L4Servo__21.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_Rotative_Base_Main_4Servo__7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_Rotative_Base_Main_4Servo__7.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_Rotative_Base_Top_4Servo__9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_Rotative_Base_Top_4Servo__9.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_Servo_Horn__12.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_Servo_Horn__12.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_Servo__8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_Servo__8.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/I_U4Servo__15.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/I_U4Servo__15.png -------------------------------------------------------------------------------- /Arduino/Resources/Shapes/RoboticArm_VRML_icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/Arduino/Resources/Shapes/RoboticArm_VRML_icon.png -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/AnalogReadSerial.ino: -------------------------------------------------------------------------------- 1 | /* 2 | AnalogReadSerial 3 | Reads an analog input on pin 0, prints the result to the serial monitor. 4 | Graphical representation is available using serial plotter (Tools > Serial Plotter menu) 5 | Attach the center pin of a potentiometer to pin A0, and the outside pins to +5V and ground. 6 | 7 | This example code is in the public domain. 8 | */ 9 | 10 | // the setup routine runs once when you press reset: 11 | void setup() { 12 | // initialize serial communication at 9600 bits per second: 13 | Serial.begin(9600); 14 | } 15 | 16 | // the loop routine runs over and over again forever: 17 | void loop() { 18 | // read the input on analog pin 0: 19 | int sensorValue = analogRead(A0); 20 | // print out the value you read: 21 | Serial.println(sensorValue); 22 | delay(1); // delay in between reads for stability 23 | } 24 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/AnalogWrite.ino: -------------------------------------------------------------------------------- 1 | 2 | const int pin = 2; 3 | 4 | void setup() { 5 | pinMode(pin, OUTPUT); 6 | } 7 | 8 | void loop() { 9 | 10 | double v = sin(millis() / 1000.0) * 1000; 11 | 12 | if (v >= 0) { 13 | 14 | pinMode(pin, OUTPUT); 15 | analogWrite(pin, map(v, 0, 1000, 0, 255)); 16 | 17 | } else { 18 | 19 | pinMode(pin, INPUT); 20 | 21 | } 22 | 23 | delay(100); 24 | 25 | } 26 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/BarGraph.ino: -------------------------------------------------------------------------------- 1 | /* 2 | LED bar graph 3 | 4 | Turns on a series of LEDs based on the value of an analog sensor. 5 | This is a simple way to make a bar graph display. Though this graph 6 | uses 10 LEDs, you can use any number by changing the LED count 7 | and the pins in the array. 8 | 9 | This method can be used to control any series of digital outputs that 10 | depends on an analog input. 11 | 12 | The circuit: 13 | * LEDs from pins 2 through 11 to ground 14 | 15 | created 4 Sep 2010 16 | by Tom Igoe 17 | 18 | This example code is in the public domain. 19 | 20 | http://www.arduino.cc/en/Tutorial/BarGraph 21 | */ 22 | 23 | 24 | // these constants won't change: 25 | const int analogPin = A0; // the pin that the potentiometer is attached to 26 | const int ledCount = 10; // the number of LEDs in the bar graph 27 | 28 | int ledPins[] = { 29 | 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 30 | }; // an array of pin numbers to which LEDs are attached 31 | 32 | 33 | void setup() { 34 | // loop over the pin array and set them all to output: 35 | for (int thisLed = 0; thisLed < ledCount; thisLed++) { 36 | pinMode(ledPins[thisLed], OUTPUT); 37 | } 38 | } 39 | 40 | void loop() { 41 | // read the potentiometer: 42 | int sensorReading = analogRead(analogPin); 43 | // map the result to a range from 0 to the number of LEDs: 44 | int ledLevel = map(sensorReading, 0, 1023, 0, ledCount); 45 | 46 | // loop over the LED array: 47 | for (int thisLed = 0; thisLed < ledCount; thisLed++) { 48 | // if the array element's index is less than ledLevel, 49 | // turn the pin for this element on: 50 | if (thisLed < ledLevel) { 51 | digitalWrite(ledPins[thisLed], HIGH); 52 | } 53 | // turn off all pins higher than the ledLevel: 54 | else { 55 | digitalWrite(ledPins[thisLed], LOW); 56 | } 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/Blink.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Blink 3 | Turns on an LED on for one second, then off for one second, repeatedly. 4 | 5 | Most Arduinos have an on-board LED you can control. On the UNO, MEGA and ZERO 6 | it is attached to digital pin 13, on MKR1000 on pin 6. LED_BUILTIN is set to 7 | the correct LED pin independent of which board is used. 8 | If you want to know what pin the on-board LED is connected to on your Arduino model, check 9 | the Technical Specs of your board at https://www.arduino.cc/en/Main/Products 10 | 11 | This example code is in the public domain. 12 | 13 | modified 8 May 2014 14 | by Scott Fitzgerald 15 | 16 | modified 2 Sep 2016 17 | by Arturo Guadalupi 18 | 19 | modified 8 Sep 2016 20 | by Colby Newman 21 | */ 22 | 23 | 24 | // the setup function runs once when you press reset or power the board 25 | void setup() { 26 | // initialize digital pin LED_BUILTIN as an output. 27 | pinMode(LED_BUILTIN, OUTPUT); 28 | } 29 | 30 | // the loop function runs over and over again forever 31 | void loop() { 32 | digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level) 33 | delay(1000); // wait for a second 34 | digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW 35 | delay(1000); // wait for a second 36 | } 37 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/BlinkWithInterrupt.ino: -------------------------------------------------------------------------------- 1 | 2 | const byte ledPin = 13; 3 | const byte interruptPin = 2; 4 | volatile byte state = LOW; 5 | 6 | void blink() { 7 | Serial.println(state); 8 | state = !state; 9 | } 10 | 11 | void setup() { 12 | pinMode(ledPin, OUTPUT); 13 | pinMode(interruptPin, INPUT); 14 | attachInterrupt(digitalPinToInterrupt(interruptPin), blink, CHANGE); 15 | } 16 | 17 | void loop() { 18 | digitalWrite(ledPin, state); 19 | } 20 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/BlinkWithoutDelay.ino: -------------------------------------------------------------------------------- 1 | /* Blink without Delay 2 | 3 | Turns on and off a light emitting diode (LED) connected to a digital 4 | pin, without using the delay() function. This means that other code 5 | can run at the same time without being interrupted by the LED code. 6 | 7 | The circuit: 8 | * LED attached from pin 13 to ground. 9 | * Note: on most Arduinos, there is already an LED on the board 10 | that's attached to pin 13, so no hardware is needed for this example. 11 | 12 | created 2005 13 | by David A. Mellis 14 | modified 8 Feb 2010 15 | by Paul Stoffregen 16 | modified 11 Nov 2013 17 | by Scott Fitzgerald 18 | 19 | 20 | This example code is in the public domain. 21 | 22 | http://www.arduino.cc/en/Tutorial/BlinkWithoutDelay 23 | */ 24 | 25 | // constants won't change. Used here to set a pin number : 26 | const int ledPin = 13; // the number of the LED pin 27 | 28 | // Variables will change : 29 | int ledState = LOW; // ledState used to set the LED 30 | 31 | // Generally, you should use "unsigned long" for variables that hold time 32 | // The value will quickly become too large for an int to store 33 | unsigned long previousMillis = 0; // will store last time LED was updated 34 | 35 | // constants won't change : 36 | const long interval = 1000; // interval at which to blink (milliseconds) 37 | 38 | void setup() { 39 | // set the digital pin as output: 40 | pinMode(ledPin, OUTPUT); 41 | } 42 | 43 | void loop() { 44 | // here is where you'd put code that needs to be running all the time. 45 | 46 | // check to see if it's time to blink the LED; that is, if the 47 | // difference between the current time and last time you blinked 48 | // the LED is bigger than the interval at which you want to 49 | // blink the LED. 50 | unsigned long currentMillis = millis(); 51 | 52 | if (currentMillis - previousMillis >= interval) { 53 | // save the last time you blinked the LED 54 | previousMillis = currentMillis; 55 | 56 | // if the LED is off turn it on and vice-versa: 57 | if (ledState == LOW) { 58 | ledState = HIGH; 59 | } else { 60 | ledState = LOW; 61 | } 62 | 63 | // set the LED with the ledState of the variable: 64 | digitalWrite(ledPin, ledState); 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/Button.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Button 3 | 4 | Turns on and off a light emitting diode(LED) connected to digital 5 | pin 13, when pressing a pushbutton attached to pin 2. 6 | 7 | 8 | The circuit: 9 | * LED attached from pin 13 to ground 10 | * pushbutton attached to pin 2 from +5V 11 | * 10K resistor attached to pin 2 from ground 12 | 13 | * Note: on most Arduinos there is already an LED on the board 14 | attached to pin 13. 15 | 16 | 17 | created 2005 18 | by DojoDave 19 | modified 30 Aug 2011 20 | by Tom Igoe 21 | 22 | This example code is in the public domain. 23 | 24 | http://www.arduino.cc/en/Tutorial/Button 25 | */ 26 | 27 | // constants won't change. They're used here to 28 | // set pin numbers: 29 | const int buttonPin = 2; // the number of the pushbutton pin 30 | const int ledPin = 13; // the number of the LED pin 31 | 32 | // variables will change: 33 | int buttonState = 0; // variable for reading the pushbutton status 34 | 35 | void setup() { 36 | // initialize the LED pin as an output: 37 | pinMode(ledPin, OUTPUT); 38 | // initialize the pushbutton pin as an input: 39 | pinMode(buttonPin, INPUT); 40 | } 41 | 42 | void loop() { 43 | // read the state of the pushbutton value: 44 | buttonState = digitalRead(buttonPin); 45 | 46 | // check if the pushbutton is pressed. 47 | // if it is, the buttonState is HIGH: 48 | if (buttonState == HIGH) { 49 | // turn LED on: 50 | digitalWrite(ledPin, HIGH); 51 | } else { 52 | // turn LED off: 53 | digitalWrite(ledPin, LOW); 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/ControlledDCMotor.ino: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #define PIN_INPUT A0 5 | #define PIN_OUTPUT 3 6 | 7 | //Define Variables we'll be connecting to 8 | double Setpoint = 10000, Input, Output; 9 | 10 | 11 | const double Kp_max = 10; 12 | const double Ki_max = 50; 13 | const double Kd_max = 1e-3; 14 | 15 | 16 | const double Setpoint_max = 12400; 17 | 18 | double Kp=0.01, Ki=4, Kd=0.00001; 19 | 20 | PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); 21 | 22 | void update() { 23 | 24 | Input = (analogRead(PIN_INPUT) / 1023.) * Setpoint_max; 25 | 26 | Kp = (analogRead(A1) / 1023.) * Kp_max; 27 | Ki = (analogRead(A2) / 1023.) * Ki_max; 28 | Kd = (analogRead(A3) / 1023.) * Kd_max; 29 | 30 | Setpoint = (analogRead(A4) / 1023.) * Setpoint_max; 31 | } 32 | 33 | void setup() { 34 | 35 | myPID.SetMode(AUTOMATIC); 36 | 37 | // set the sample rate to 1 ms 38 | myPID.SetSampleTime(1); 39 | } 40 | 41 | void loop() { 42 | 43 | //ModelicaFormatMessage("Input=%g, Kp=%g, Ki=%g, Kd=%g, Setpoint=%g\n", Input, Kp, Ki, Kd, Setpoint); 44 | 45 | update(); 46 | 47 | myPID.Compute(); 48 | 49 | analogWrite(PIN_OUTPUT, Output); 50 | } 51 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/Fade.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Fading 3 | 4 | This example shows how to fade an LED using the analogWrite() function. 5 | 6 | The circuit: 7 | * LED attached from digital pin 9 to ground. 8 | 9 | Created 1 Nov 2008 10 | By David A. Mellis 11 | modified 30 Aug 2011 12 | By Tom Igoe 13 | 14 | http://www.arduino.cc/en/Tutorial/Fading 15 | 16 | This example code is in the public domain. 17 | 18 | */ 19 | 20 | 21 | int ledPin = 9; // LED connected to digital pin 9 22 | 23 | void setup() { 24 | // nothing happens in setup 25 | } 26 | 27 | void loop() { 28 | // fade in from min to max in increments of 5 points: 29 | for (int fadeValue = 0 ; fadeValue <= 255; fadeValue += 5) { 30 | // sets the value (range from 0 to 255): 31 | analogWrite(ledPin, fadeValue); 32 | // wait for 30 milliseconds to see the dimming effect 33 | delay(30); 34 | } 35 | 36 | // fade out from max to min in increments of 5 points: 37 | for (int fadeValue = 255 ; fadeValue >= 0; fadeValue -= 5) { 38 | // sets the value (range from 0 to 255): 39 | analogWrite(ledPin, fadeValue); 40 | // wait for 30 milliseconds to see the dimming effect 41 | delay(30); 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/Functions.ino: -------------------------------------------------------------------------------- 1 | /* Functions Test */ 2 | 3 | void setup() { 4 | 5 | pinMode(LED_BUILTIN, OUTPUT); 6 | 7 | 8 | Serial.println(); 9 | Serial.println("Digital I/O"); 10 | Serial.println(); 11 | 12 | Serial.println("pinMode(0, INPUT)"); 13 | pinMode(0, INPUT); 14 | 15 | Serial.print("digitalRead(0) = "); 16 | Serial.println(digitalRead(0)); 17 | 18 | Serial.println("pinMode(0, INPUT)"); 19 | pinMode(0, INPUT); 20 | 21 | Serial.println("digitalWrite(0, HIGH)"); 22 | digitalWrite(0, HIGH); 23 | 24 | 25 | Serial.println(); 26 | Serial.println("Analog I/O"); 27 | Serial.println(); 28 | 29 | Serial.println("analogReference(DEFAULT)"); 30 | analogReference(DEFAULT); 31 | 32 | Serial.print("analogRead(A0)"); 33 | Serial.println(analogRead(A0)); 34 | 35 | Serial.println("analogWrite(9, 100)"); 36 | analogWrite(9, 100); 37 | 38 | 39 | Serial.println(); 40 | Serial.println("Math"); 41 | Serial.println(); 42 | 43 | Serial.print("min(0, 1) = "); 44 | Serial.println(min(0, 1)); 45 | 46 | Serial.print("max(0, 1) = "); 47 | Serial.println(max(0, 1)); 48 | 49 | Serial.print("abs(-2) = "); 50 | Serial.println(abs(-2)); 51 | 52 | Serial.print("constrain(1, 10, 100) = "); 53 | Serial.println(constrain(1, 10, 100)); 54 | 55 | Serial.print("map(11, 1, 50, 50, 1) = "); 56 | Serial.println(map(11, 1, 50, 50, 1)); 57 | 58 | Serial.print("pow(2., 3.) = "); 59 | Serial.println(pow(2., 3.)); 60 | 61 | Serial.print("sqrt(81.) = "); 62 | Serial.println(sqrt(81.)); 63 | 64 | 65 | Serial.println(); 66 | Serial.println("Trigonometry"); 67 | Serial.println(); 68 | 69 | Serial.print("sin(1.) = "); 70 | Serial.println(sin(1.)); 71 | 72 | Serial.print("cos(1.) = "); 73 | Serial.println(cos(1.)); 74 | 75 | Serial.print("tan(1.) = "); 76 | Serial.println(tan(1.)); 77 | 78 | 79 | Serial.println(); 80 | Serial.println("Characters"); 81 | Serial.println(); 82 | 83 | Serial.print("isAlphaNumeric('a') = "); 84 | Serial.println(isAlphaNumeric('a')); 85 | 86 | Serial.print("isAlpha('a') = "); 87 | Serial.println(isAlpha('a')); 88 | 89 | Serial.print("isAscii('a') = "); 90 | Serial.println(isAscii('a')); 91 | 92 | Serial.print("isWhitespace('a') = "); 93 | Serial.println(isWhitespace('a')); 94 | 95 | Serial.print("isControl('a') = "); 96 | Serial.println(isControl('a')); 97 | 98 | Serial.print("isDigit('a') = "); 99 | Serial.println(isDigit('a')); 100 | 101 | Serial.print("isGraph('a') = "); 102 | Serial.println(isGraph('a')); 103 | 104 | Serial.print("isLowerCase('a') = "); 105 | Serial.println(isLowerCase('a')); 106 | 107 | Serial.print("isPrintable('a') = "); 108 | Serial.println(isPrintable('a')); 109 | 110 | Serial.print("isPunct('a') = "); 111 | Serial.println(isPunct('a')); 112 | 113 | Serial.print("isSpace('a') = "); 114 | Serial.println(isSpace('a')); 115 | 116 | Serial.print("isUpperCase('a') = "); 117 | Serial.println(isUpperCase('a')); 118 | 119 | Serial.print("isHexadecimalDigit('a') = "); 120 | Serial.println(isHexadecimalDigit('a')); 121 | 122 | 123 | Serial.println(); 124 | Serial.println("Random Numbers"); 125 | Serial.println(); 126 | 127 | Serial.println("randomSeed(1)"); 128 | randomSeed(1); 129 | 130 | Serial.print("random(10, 20) = "); 131 | Serial.println(random(10, 20)); 132 | 133 | 134 | Serial.println(); 135 | Serial.println("Bits and Bytes"); 136 | Serial.println(); 137 | 138 | Serial.print("lowByte(1) = "); 139 | Serial.println(lowByte(1)); 140 | 141 | Serial.print("highByte(1) = "); 142 | Serial.println(highByte(1)); 143 | 144 | int x; 145 | 146 | Serial.print("bitRead(x, 1) = "); 147 | Serial.println(bitRead(x, 1)); 148 | 149 | Serial.println("bitWrite(x, 1, 1)"); 150 | bitWrite(x, 1, 1); 151 | 152 | Serial.println("bitSet(x, 1)"); 153 | bitSet(x, 1); 154 | 155 | Serial.println("bitClear(x, 1)"); 156 | bitClear(x, 1); 157 | 158 | Serial.print("bit(3) = "); 159 | Serial.println(bit(3)); 160 | 161 | Serial.println(); 162 | 163 | } 164 | 165 | void loop() { 166 | digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level) 167 | delay(1000); // wait for a second 168 | digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW 169 | delay(1000); // wait for a second 170 | } 171 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/LEDMatrix.ino: -------------------------------------------------------------------------------- 1 | #define ROW_1 9 2 | #define ROW_2 4 3 | #define ROW_3 A2 4 | #define ROW_4 6 5 | #define ROW_5 10 6 | #define ROW_6 A3 7 | #define ROW_7 11 8 | #define ROW_8 A5 9 | 10 | #define COL_1 5 11 | #define COL_2 12 12 | #define COL_3 13 13 | #define COL_4 8 14 | #define COL_5 A4 15 | #define COL_6 7 16 | #define COL_7 3 17 | #define COL_8 2 18 | 19 | const byte rows[] = { 20 | ROW_1, ROW_2, ROW_3, ROW_4, ROW_5, ROW_6, ROW_7, ROW_8 21 | }; 22 | 23 | const byte odd[] = { 24 | B11111111, 25 | B00000000, 26 | B11111111, 27 | B00000000, 28 | B11111111, 29 | B00000000, 30 | B11111111, 31 | B00000000, 32 | }; 33 | 34 | const byte even[] = { 35 | B00000000, 36 | B11111111, 37 | B00000000, 38 | B11111111, 39 | B00000000, 40 | B11111111, 41 | B00000000, 42 | B11111111 43 | }; 44 | 45 | void setColumns(byte b) { 46 | digitalWrite(COL_1, (~b >> 7) & 0x01); // Get the 1st bit: 10000000 47 | digitalWrite(COL_2, (~b >> 6) & 0x01); // Get the 2nd bit: 01000000 48 | digitalWrite(COL_3, (~b >> 5) & 0x01); // Get the 3rd bit: 00100000 49 | digitalWrite(COL_4, (~b >> 4) & 0x01); // Get the 4th bit: 00010000 50 | digitalWrite(COL_5, (~b >> 3) & 0x01); // Get the 5th bit: 00001000 51 | digitalWrite(COL_6, (~b >> 2) & 0x01); // Get the 6th bit: 00000100 52 | digitalWrite(COL_7, (~b >> 1) & 0x01); // Get the 7th bit: 00000010 53 | digitalWrite(COL_8, (~b >> 0) & 0x01); // Get the 8th bit: 00000001 54 | } 55 | 56 | void drawScreen(const byte buffer[]){ 57 | 58 | for (byte i = 0; i < 8; i++) { 59 | setColumns(buffer[i]); 60 | digitalWrite(rows[i], HIGH); 61 | delay(10); 62 | digitalWrite(rows[i], LOW); 63 | } 64 | } 65 | 66 | void setup() { 67 | pinMode(ROW_1, OUTPUT); 68 | pinMode(ROW_2, OUTPUT); 69 | pinMode(ROW_3, OUTPUT); 70 | pinMode(ROW_4, OUTPUT); 71 | pinMode(ROW_5, OUTPUT); 72 | pinMode(ROW_6, OUTPUT); 73 | pinMode(ROW_7, OUTPUT); 74 | pinMode(ROW_8, OUTPUT); 75 | 76 | pinMode(COL_1, OUTPUT); 77 | pinMode(COL_2, OUTPUT); 78 | pinMode(COL_3, OUTPUT); 79 | pinMode(COL_4, OUTPUT); 80 | pinMode(COL_5, OUTPUT); 81 | pinMode(COL_6, OUTPUT); 82 | pinMode(COL_7, OUTPUT); 83 | pinMode(COL_8, OUTPUT); 84 | } 85 | 86 | void loop() { 87 | 88 | static int frameCount = 0; 89 | 90 | if(frameCount < 10) { 91 | drawScreen(odd); 92 | } else if (frameCount < 20) { 93 | drawScreen(even); 94 | } else { 95 | frameCount = 0; 96 | } 97 | 98 | frameCount++; 99 | } 100 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/Melody.ino: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Melody 4 | 5 | Plays a melody 6 | 7 | circuit: 8 | 9 | - 8 ohm speaker on digital pin 8 10 | 11 | created 21 Jan 2010 12 | 13 | modified 30 Aug 2011 14 | 15 | by Tom Igoe 16 | 17 | This example code is in the public domain. 18 | 19 | http://www.arduino.cc/en/Tutorial/Tone 20 | 21 | */ 22 | 23 | #include "pitches.h" 24 | 25 | // notes in the melody: 26 | int melody[] = { 27 | 28 | NOTE_C4, NOTE_G3, NOTE_G3, NOTE_A3, NOTE_G3, 0, NOTE_B3, NOTE_C4 29 | }; 30 | 31 | // note durations: 4 = quarter note, 8 = eighth note, etc.: 32 | int noteDurations[] = { 33 | 34 | 4, 8, 8, 4, 4, 4, 4, 4 35 | }; 36 | 37 | void setup() { 38 | 39 | // iterate over the notes of the melody: 40 | 41 | for (int thisNote = 0; thisNote < 8; thisNote++) { 42 | 43 | // to calculate the note duration, take one second divided by the note type. 44 | 45 | //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc. 46 | 47 | int noteDuration = 1000 / noteDurations[thisNote]; 48 | 49 | tone(8, melody[thisNote], noteDuration); 50 | 51 | // to distinguish the notes, set a minimum time between them. 52 | 53 | // the note's duration + 30% seems to work well: 54 | 55 | int pauseBetweenNotes = noteDuration * 1.30; 56 | 57 | delay(pauseBetweenNotes); 58 | 59 | // stop the tone playing: 60 | 61 | noTone(8); 62 | 63 | } 64 | } 65 | 66 | void loop() { 67 | 68 | // no need to repeat the melody. 69 | } -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/Ping.ino: -------------------------------------------------------------------------------- 1 | /* Ping))) Sensor 2 | 3 | This sketch reads a PING))) ultrasonic rangefinder and returns the 4 | distance to the closest object in range. To do this, it sends a pulse 5 | to the sensor to initiate a reading, then listens for a pulse 6 | to return. The length of the returning pulse is proportional to 7 | the distance of the object from the sensor. 8 | 9 | The circuit: 10 | * +V connection of the PING))) attached to +5V 11 | * GND connection of the PING))) attached to ground 12 | * SIG connection of the PING))) attached to digital pin 7 13 | 14 | http://www.arduino.cc/en/Tutorial/Ping 15 | 16 | created 3 Nov 2008 17 | by David A. Mellis 18 | modified 30 Aug 2011 19 | by Tom Igoe 20 | 21 | This example code is in the public domain. 22 | 23 | */ 24 | 25 | // this constant won't change. It's the pin number 26 | // of the sensor's output: 27 | const int pingPin = 7; 28 | 29 | long microsecondsToInches(long microseconds) { 30 | // According to Parallax's datasheet for the PING))), there are 31 | // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per 32 | // second). This gives the distance travelled by the ping, outbound 33 | // and return, so we divide by 2 to get the distance of the obstacle. 34 | // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf 35 | return microseconds / 74 / 2; 36 | } 37 | 38 | long microsecondsToCentimeters(long microseconds) { 39 | // The speed of sound is 340 m/s or 29 microseconds per centimeter. 40 | // The ping travels out and back, so to find the distance of the 41 | // object we take half of the distance travelled. 42 | return microseconds / 29 / 2; 43 | } 44 | 45 | void setup() { 46 | // initialize serial communication: 47 | Serial.begin(9600); 48 | } 49 | 50 | void loop() { 51 | // establish variables for duration of the ping, 52 | // and the distance result in inches and centimeters: 53 | long duration, inches, cm; 54 | 55 | // The PING))) is triggered by a HIGH pulse of 2 or more microseconds. 56 | // Give a short LOW pulse beforehand to ensure a clean HIGH pulse: 57 | pinMode(pingPin, OUTPUT); 58 | digitalWrite(pingPin, LOW); 59 | delayMicroseconds(2); 60 | digitalWrite(pingPin, HIGH); 61 | delayMicroseconds(50); 62 | digitalWrite(pingPin, LOW); 63 | 64 | // The same pin is used to read the signal from the PING))): a HIGH 65 | // pulse whose duration is the time (in microseconds) from the sending 66 | // of the ping to the reception of its echo off of an object. 67 | pinMode(pingPin, INPUT); 68 | duration = pulseIn(pingPin, HIGH); 69 | 70 | // convert the time into a distance 71 | inches = microsecondsToInches(duration); 72 | cm = microsecondsToCentimeters(duration); 73 | 74 | 75 | Serial.print("duration: "); 76 | Serial.print(duration); 77 | Serial.println(" us"); 78 | 79 | Serial.print(inches); 80 | Serial.print("in, "); 81 | Serial.print(cm); 82 | Serial.print("cm"); 83 | Serial.println(); 84 | 85 | delay(100); 86 | } 87 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/PulseInExample.ino: -------------------------------------------------------------------------------- 1 | int pin = 2; 2 | 3 | unsigned long duration; 4 | 5 | void setup() { 6 | pinMode(pin, INPUT); 7 | } 8 | 9 | void loop() { 10 | duration = pulseIn(pin, HIGH); 11 | Serial.println(duration); 12 | } 13 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/RobotArmGrabSponge.ino: -------------------------------------------------------------------------------- 1 | #include "DFLG6DOF.h" 2 | 3 | RobotArm robotArm; 4 | 5 | 6 | void setup() { 7 | 8 | /* 9 | robotArm.baseAngleOffset = -10; 10 | robotArm.elbowAngleOffset = -13; 11 | robotArm.wristVerAngleOffset = -7; 12 | robotArm.wristRotAngleOffset = -3; 13 | robotArm.gripperAngleOffset = 0; 14 | 15 | robotArm.reverseBaseAngle = true; 16 | robotArm.reverseWristVerAngle = true; 17 | robotArm.reverseWristRotAngle = true; 18 | */ 19 | 20 | robotArm.begin(); 21 | 22 | } 23 | 24 | void loop() { 25 | 26 | delay(1000); 27 | 28 | // shoulder up 29 | robotArm.shoulderAngle = 160; 30 | robotArm.elbowAngle = 30; 31 | robotArm.wristVerAngle = 125; 32 | robotArm.move(); 33 | 34 | // turn base and wrist 35 | robotArm.baseAngle = 145; 36 | robotArm.wristRotAngle = 180; 37 | robotArm.move(); 38 | 39 | // arm down 40 | robotArm.shoulderAngle = 70; 41 | robotArm.move(); 42 | 43 | // close gripper 44 | robotArm.gripperAngle = 70; 45 | robotArm.move(); 46 | 47 | // arm up 48 | robotArm.shoulderAngle = 160; 49 | //robotArm.elbowAngle = 10; 50 | robotArm.move(); 51 | 52 | // turn back base and wrist 53 | robotArm.baseAngle = 35; 54 | robotArm.wristRotAngle = 90; 55 | robotArm.move(); 56 | 57 | // shoulder down 58 | robotArm.shoulderAngle = 70; 59 | robotArm.move(); 60 | 61 | // open gripper 62 | robotArm.gripperAngle = 0; 63 | robotArm.move(); 64 | 65 | // arm up 66 | robotArm.shoulderAngle = 160; 67 | robotArm.move(); 68 | 69 | // turn back base 70 | robotArm.baseAngle = 90; 71 | robotArm.move(); 72 | 73 | // arm down 74 | robotArm.shoulderAngle = 80; 75 | robotArm.elbowAngle = 5; 76 | robotArm.wristVerAngle = WRIST_VER_START_ANGLE; 77 | robotArm.move(); 78 | 79 | delay(10000); 80 | 81 | } 82 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/ShftOut11.ino: -------------------------------------------------------------------------------- 1 | //**************************************************************// 2 | // Name : shiftOutCode, Hello World 3 | // Author : Carlyn Maw,Tom Igoe, David A. Mellis 4 | // Date : 25 Oct, 2006 5 | // Modified: 23 Mar 2010 6 | // Version : 2.0 7 | // Notes : Code for using a 74HC595 Shift Register // 8 | // : to count from 0 to 255 9 | //**************************************************************** 10 | //Pin connected to ST_CP of 74HC595 11 | int latchPin = 8; 12 | //Pin connected to SH_CP of 74HC595 13 | int clockPin = 12; 14 | ////Pin connected to DS of 74HC595 15 | int dataPin = 11; 16 | void setup() { 17 | //set pins to output so you can control the shift register 18 | pinMode(latchPin, OUTPUT); 19 | pinMode(clockPin, OUTPUT); 20 | pinMode(dataPin, OUTPUT); 21 | } 22 | void loop() { 23 | // count from 0 to 255 and display the number 24 | // on the LEDs 25 | for (int numberToDisplay = 0; numberToDisplay < 256; numberToDisplay++) { 26 | // take the latchPin low so 27 | // the LEDs don't change while you're sending in bits: 28 | digitalWrite(latchPin, LOW); 29 | // shift out the bits: 30 | shiftOut(dataPin, clockPin, MSBFIRST, numberToDisplay); 31 | //take the latch pin high so the LEDs will light up: 32 | digitalWrite(latchPin, HIGH); 33 | // pause before next value: 34 | delay(500); 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/Sweep.ino: -------------------------------------------------------------------------------- 1 | /* Sweep 2 | by BARRAGAN 3 | This example code is in the public domain. 4 | 5 | modified 8 Nov 2013 6 | by Scott Fitzgerald 7 | http://www.arduino.cc/en/Tutorial/Sweep 8 | */ 9 | 10 | #include 11 | 12 | Servo myservo; // create servo object to control a servo 13 | // twelve servo objects can be created on most boards 14 | 15 | int pos = 0; // variable to store the servo position 16 | 17 | void setup() { 18 | myservo.attach(9); // attaches the servo on pin 9 to the servo object 19 | } 20 | 21 | void loop() { 22 | for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees 23 | // in steps of 1 degree 24 | myservo.write(pos); // tell servo to go to position in variable 'pos' 25 | delay(15); // waits 15ms for the servo to reach the position 26 | } 27 | for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees 28 | myservo.write(pos); // tell servo to go to position in variable 'pos' 29 | delay(15); // waits 15ms for the servo to reach the position 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /Arduino/Resources/Sketches/pitches.h: -------------------------------------------------------------------------------- 1 | /************************************************* 2 | 3 | * Public Constants 4 | 5 | *************************************************/ 6 | 7 | #define NOTE_B0 31 8 | #define NOTE_C1 33 9 | #define NOTE_CS1 35 10 | #define NOTE_D1 37 11 | #define NOTE_DS1 39 12 | #define NOTE_E1 41 13 | #define NOTE_F1 44 14 | #define NOTE_FS1 46 15 | #define NOTE_G1 49 16 | #define NOTE_GS1 52 17 | #define NOTE_A1 55 18 | #define NOTE_AS1 58 19 | #define NOTE_B1 62 20 | #define NOTE_C2 65 21 | #define NOTE_CS2 69 22 | #define NOTE_D2 73 23 | #define NOTE_DS2 78 24 | #define NOTE_E2 82 25 | #define NOTE_F2 87 26 | #define NOTE_FS2 93 27 | #define NOTE_G2 98 28 | #define NOTE_GS2 104 29 | #define NOTE_A2 110 30 | #define NOTE_AS2 117 31 | #define NOTE_B2 123 32 | #define NOTE_C3 131 33 | #define NOTE_CS3 139 34 | #define NOTE_D3 147 35 | #define NOTE_DS3 156 36 | #define NOTE_E3 165 37 | #define NOTE_F3 175 38 | #define NOTE_FS3 185 39 | #define NOTE_G3 196 40 | #define NOTE_GS3 208 41 | #define NOTE_A3 220 42 | #define NOTE_AS3 233 43 | #define NOTE_B3 247 44 | #define NOTE_C4 262 45 | #define NOTE_CS4 277 46 | #define NOTE_D4 294 47 | #define NOTE_DS4 311 48 | #define NOTE_E4 330 49 | #define NOTE_F4 349 50 | #define NOTE_FS4 370 51 | #define NOTE_G4 392 52 | #define NOTE_GS4 415 53 | #define NOTE_A4 440 54 | #define NOTE_AS4 466 55 | #define NOTE_B4 494 56 | #define NOTE_C5 523 57 | #define NOTE_CS5 554 58 | #define NOTE_D5 587 59 | #define NOTE_DS5 622 60 | #define NOTE_E5 659 61 | #define NOTE_F5 698 62 | #define NOTE_FS5 740 63 | #define NOTE_G5 784 64 | #define NOTE_GS5 831 65 | #define NOTE_A5 880 66 | #define NOTE_AS5 932 67 | #define NOTE_B5 988 68 | #define NOTE_C6 1047 69 | #define NOTE_CS6 1109 70 | #define NOTE_D6 1175 71 | #define NOTE_DS6 1245 72 | #define NOTE_E6 1319 73 | #define NOTE_F6 1397 74 | #define NOTE_FS6 1480 75 | #define NOTE_G6 1568 76 | #define NOTE_GS6 1661 77 | #define NOTE_A6 1760 78 | #define NOTE_AS6 1865 79 | #define NOTE_B6 1976 80 | #define NOTE_C7 2093 81 | #define NOTE_CS7 2217 82 | #define NOTE_D7 2349 83 | #define NOTE_DS7 2489 84 | #define NOTE_E7 2637 85 | #define NOTE_F7 2794 86 | #define NOTE_FS7 2960 87 | #define NOTE_G7 3136 88 | #define NOTE_GS7 3322 89 | #define NOTE_A7 3520 90 | #define NOTE_AS7 3729 91 | #define NOTE_B7 3951 92 | #define NOTE_C8 4186 93 | #define NOTE_CS8 4435 94 | #define NOTE_D8 4699 95 | #define NOTE_DS8 4978 -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 3.15) 2 | 3 | set(CMAKE_MSVC_RUNTIME_LIBRARY "MultiThreaded$<$:Debug>") 4 | 5 | project (Arduino) 6 | 7 | if ("${CMAKE_SIZEOF_VOID_P}" STREQUAL "8") 8 | set(PLATFORM win64) 9 | else () 10 | set(PLATFORM win32) 11 | endif () 12 | 13 | file(GLOB SRC_FILES *.h *.cpp) 14 | 15 | add_library(ModelicaArduino SHARED ${SRC_FILES}) 16 | 17 | target_compile_definitions(ModelicaArduino PUBLIC 18 | ARDUINO=100 19 | ) 20 | 21 | SET_TARGET_PROPERTIES(ModelicaArduino PROPERTIES PREFIX "") 22 | 23 | target_include_directories(ModelicaArduino PUBLIC 24 | . 25 | ../../Include 26 | ../../Sketches 27 | ) 28 | 29 | add_custom_command(TARGET ModelicaArduino POST_BUILD COMMAND ${CMAKE_COMMAND} -E copy 30 | "$" 31 | "${CMAKE_CURRENT_SOURCE_DIR}/../../Library/${PLATFORM}" 32 | ) 33 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/DFLG6DOF.h: -------------------------------------------------------------------------------- 1 | #ifndef DFLG6DOF_H 2 | #define DFLG6DOF_H 3 | 4 | #include 5 | 6 | // gripper 0: open, 155: closed 7 | // wristRot 0: right, 90: centered, 180 left 8 | // wristVer 0: straight, 90: down 9 | // elbow 30: down, 110: straight 10 | // shoulder 60: 45 deg forward, 90: straight up, 140: 45 deg backward 11 | // base 0: away from circles, 90: to large circle, 180: between circles 12 | 13 | 14 | #define BASE_START_ANGLE 90 15 | #define SHOULDER_START_ANGLE 80 16 | #define ELBOW_START_ANGLE 5 17 | #define WRIST_VER_START_ANGLE 140 18 | #define WRIST_ROT_START_ANGLE 90 19 | #define GRIPPER_START_ANGLE 0 20 | 21 | 22 | int clamp(int value, int vmin, int vmax) { 23 | if (value < vmin) return vmin; 24 | if (value > vmax) return vmax; 25 | return value; 26 | } 27 | 28 | 29 | class RobotArm { 30 | 31 | public: 32 | 33 | Servo baseServo; 34 | Servo shoulderServo; 35 | Servo elbowServo; 36 | Servo wristVerServo; 37 | Servo wristRotServo; 38 | Servo gripperServo; 39 | 40 | int stepDelay = 20; 41 | 42 | // target angles 43 | int baseAngle = BASE_START_ANGLE; 44 | int shoulderAngle = SHOULDER_START_ANGLE; 45 | int elbowAngle = ELBOW_START_ANGLE; 46 | int wristVerAngle = WRIST_VER_START_ANGLE; 47 | int wristRotAngle = WRIST_ROT_START_ANGLE; 48 | int gripperAngle = GRIPPER_START_ANGLE; 49 | 50 | // current angles 51 | int m_baseAngle; 52 | int m_shoulderAngle; 53 | int m_elbowAngle; 54 | int m_wristVerAngle; 55 | int m_wristRotAngle; 56 | int m_gripperAngle; 57 | 58 | // offsets for clibration 59 | int baseAngleOffset = 0; 60 | int shoulderAngleOffset = 0; 61 | int elbowAngleOffset = 0; 62 | int wristVerAngleOffset = 0; 63 | int wristRotAngleOffset = 0; 64 | int gripperAngleOffset = 0; 65 | 66 | // factor 67 | bool reverseBaseAngle = false; 68 | bool reverseShoulderAngle = false; 69 | bool reverseElbowAngle = false; 70 | bool reverseWristVerAngle = false; 71 | bool reverseWristRotAngle = false; 72 | bool reverseGripperAngle = false; 73 | 74 | void begin() { 75 | 76 | // attach the servos 77 | baseServo.attach(3); 78 | shoulderServo.attach(5); 79 | elbowServo.attach(6); 80 | wristVerServo.attach(9); 81 | wristRotServo.attach(10); 82 | gripperServo.attach(11); 83 | 84 | m_baseAngle = baseAngle; 85 | m_shoulderAngle = shoulderAngle; 86 | m_elbowAngle = elbowAngle; 87 | m_wristVerAngle = wristVerAngle; 88 | m_wristRotAngle = wristRotAngle; 89 | m_gripperAngle = gripperAngle; 90 | 91 | // move to start position 92 | baseServo.write(servoValue(m_baseAngle, reverseBaseAngle, baseAngleOffset)); 93 | shoulderServo.write(servoValue(m_shoulderAngle, reverseShoulderAngle, shoulderAngleOffset)); 94 | elbowServo.write(servoValue(m_elbowAngle, reverseElbowAngle, elbowAngleOffset)); 95 | wristVerServo.write(servoValue(m_wristVerAngle, reverseWristVerAngle, wristVerAngleOffset)); 96 | wristRotServo.write(servoValue(m_wristRotAngle, reverseWristRotAngle, wristRotAngleOffset)); 97 | gripperServo.write(servoValue(m_gripperAngle, reverseGripperAngle, gripperAngleOffset)); 98 | 99 | } 100 | 101 | int servoValue(int value, bool reverse, int offset) { 102 | 103 | if (reverse) { 104 | return 180 - value + offset; 105 | } else { 106 | return value + offset; 107 | } 108 | 109 | } 110 | 111 | void move() { 112 | 113 | /* 114 | baseAngle = clamp(baseAngle, 0, 180); 115 | shoulderAngle = clamp(shoulderAngle, 60, 140); 116 | elbowAngle = clamp(elbowAngle, 0, 110); 117 | wristVerAngle = clamp(wristVerAngle, 0, 130); 118 | wristRotAngle = clamp(wristRotAngle, 0, 180); 119 | gripperAngle = clamp(gripperAngle, 0, 140); 120 | */ 121 | 122 | bool done = false; 123 | 124 | while (!done) { 125 | 126 | done = true; 127 | 128 | if (m_baseAngle < baseAngle) { 129 | m_baseAngle++; 130 | done = false; 131 | } else if (m_baseAngle > baseAngle) { 132 | m_baseAngle--; 133 | done = false; 134 | } 135 | 136 | if (m_shoulderAngle < shoulderAngle) { 137 | m_shoulderAngle++; 138 | done = false; 139 | } else if (m_shoulderAngle > shoulderAngle) { 140 | m_shoulderAngle--; 141 | done = false; 142 | } 143 | 144 | if (m_elbowAngle < elbowAngle) { 145 | m_elbowAngle++; 146 | done = false; 147 | } else if (m_elbowAngle > elbowAngle) { 148 | m_elbowAngle--; 149 | done = false; 150 | } 151 | 152 | if (m_wristVerAngle < wristVerAngle) { 153 | m_wristVerAngle++; 154 | done = false; 155 | } else if (m_wristVerAngle > wristVerAngle) { 156 | m_wristVerAngle--; 157 | done = false; 158 | } 159 | 160 | if (m_wristRotAngle < wristRotAngle) { 161 | m_wristRotAngle++; 162 | done = false; 163 | } else if (m_wristRotAngle > wristRotAngle) { 164 | m_wristRotAngle--; 165 | done = false; 166 | } 167 | 168 | if (m_gripperAngle < gripperAngle) { 169 | m_gripperAngle++; 170 | done = false; 171 | } else if (m_gripperAngle > gripperAngle) { 172 | m_gripperAngle--; 173 | done = false; 174 | } 175 | 176 | baseServo.write(servoValue(m_baseAngle, reverseBaseAngle, baseAngleOffset)); 177 | shoulderServo.write(servoValue(m_shoulderAngle, reverseShoulderAngle, shoulderAngleOffset)); 178 | elbowServo.write(servoValue(m_elbowAngle, reverseElbowAngle, elbowAngleOffset)); 179 | wristVerServo.write(servoValue(m_wristVerAngle, reverseWristVerAngle, wristVerAngleOffset)); 180 | wristRotServo.write(servoValue(m_wristRotAngle, reverseWristRotAngle, wristRotAngleOffset)); 181 | gripperServo.write(servoValue(m_gripperAngle, reverseGripperAngle, gripperAngleOffset)); 182 | 183 | delay(stepDelay); 184 | } 185 | 186 | } 187 | 188 | }; 189 | 190 | #endif /* DFLG6DOF_H */ 191 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/Event.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | class Event { 9 | 10 | public: 11 | 12 | Event(bool initial, bool manual) : 13 | state(initial), 14 | manual(manual) {} 15 | 16 | void change(bool manual) { 17 | 18 | std::unique_lock lock(mutex); 19 | 20 | this->manual = manual; 21 | } 22 | 23 | void set() { 24 | 25 | std::unique_lock lock(mutex); 26 | 27 | if (state) { 28 | return; 29 | } 30 | 31 | state = true; 32 | 33 | if (manual) { 34 | condition.notify_all(); 35 | } else { 36 | condition.notify_one(); 37 | } 38 | } 39 | 40 | void reset() { 41 | 42 | std::unique_lock lock(mutex); 43 | 44 | state = false; 45 | } 46 | 47 | void wait() { 48 | 49 | std::unique_lock lock(mutex); 50 | 51 | condition.wait(lock, [this] { return state; }); 52 | 53 | if (!manual) { 54 | state = false; 55 | } 56 | } 57 | 58 | template 59 | void wait(const std::chrono::duration& timeout) { 60 | 61 | std::unique_lock lock(mutex); 62 | 63 | if (!condition.wait_for(lock, timeout, [this] { return state; })) { 64 | return; 65 | } 66 | 67 | if (!manual) { 68 | state = false; 69 | } 70 | } 71 | 72 | private: 73 | 74 | std::mutex mutex; 75 | std::condition_variable condition; 76 | bool state, manual; 77 | }; 78 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/ModelicaArduino.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "ModelicaArduino.h" 6 | #include "ModelicaUtilities.h" 7 | #include "ModelicaUtilityFunctions.h" 8 | 9 | #include "Arduino.h" 10 | #include 11 | #include "SoftArduino.h" 12 | 13 | 14 | /** Constructor function of the external object */ 15 | void * ModelicaArduino_open(void *callbacks) { 16 | 17 | setModelicaUtilityFunctions(reinterpret_cast(callbacks)); 18 | 19 | return INSTANCE; 20 | } 21 | 22 | /** Destructor function of the external object */ 23 | void ModelicaArduino_close(void *externalObject) { 24 | //ModelicaMessage("ModelicaArduino_close()\n"); 25 | } 26 | 27 | void ModelicaArduino_update(void *instance__, 28 | double time, 29 | double analogReference, 30 | double *potential, 31 | int *portMode, 32 | int *pulseWidth, 33 | int *pulsePeriod) { 34 | 35 | if (INSTANCE->error) { 36 | ModelicaFormatError("Error in loop(): %s", INSTANCE->error); 37 | return; 38 | } 39 | 40 | if (INSTANCE->analogReferenceMode == EXTERNAL && analogReference == 0) { 41 | ModelicaFormatError("Reference voltage must not be 0."); 42 | return; 43 | } 44 | 45 | // update the time 46 | INSTANCE->time = static_cast(time * 1e6); 47 | 48 | const double V_ref = INSTANCE->analogReferenceMode == EXTERNAL ? analogReference : 5.; 49 | 50 | for (int i = 0; i < NUM_DIGITAL_PINS; i++) { 51 | INSTANCE->potential[i] = potential[i] / V_ref; 52 | } 53 | 54 | INSTANCE->update(); 55 | 56 | for (int i = 0; i < NUM_DIGITAL_PINS; i++) { 57 | portMode[i] = INSTANCE->portMode[i]; 58 | pulseWidth[i] = INSTANCE->pulseWidth[i]; 59 | pulsePeriod[i] = INSTANCE->pulsePeriod[i]; 60 | } 61 | 62 | // interrupts 63 | if (INSTANCE->interruptsEnabled) { 64 | for (int i = 0; i < 2; i++) { 65 | if (INSTANCE->interrupts[i]) { 66 | const int p = INSTANCE->potential[interruptToDigitalPin(i)] > 0.5 ? HIGH : LOW; 67 | INSTANCE->interrupts[i]->update(p); 68 | } 69 | } 70 | } 71 | 72 | } 73 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/ModelicaUtilities.cpp: -------------------------------------------------------------------------------- 1 | #include "ModelicaUtilities.h" 2 | #include "ModelicaUtilityFunctions.h" 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | static ModelicaUtilityFunctions_t s_callbacks = { nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr }; 9 | 10 | 11 | void setModelicaUtilityFunctions(ModelicaUtilityFunctions_t *callbacks) { 12 | s_callbacks = *callbacks; 13 | } 14 | 15 | 16 | void ModelicaMessage(const char *string) { 17 | 18 | if (s_callbacks.ModelicaMessage) { 19 | s_callbacks.ModelicaMessage(string); 20 | } else { 21 | printf(string); 22 | } 23 | } 24 | 25 | 26 | void ModelicaFormatMessage(const char *string, ...) { 27 | 28 | va_list vl; 29 | va_start(vl, string); 30 | 31 | if (s_callbacks.ModelicaVFormatMessage) { 32 | s_callbacks.ModelicaVFormatMessage(string, vl); 33 | } else { 34 | vprintf(string, vl); 35 | } 36 | 37 | va_end(vl); 38 | } 39 | 40 | 41 | void ModelicaVFormatMessage(const char *string, va_list vl) { 42 | 43 | if (s_callbacks.ModelicaVFormatMessage) { 44 | s_callbacks.ModelicaVFormatMessage(string, vl); 45 | } else { 46 | vprintf(string, vl); 47 | } 48 | } 49 | 50 | 51 | void ModelicaError(const char *string) { 52 | 53 | if (s_callbacks.ModelicaError) { 54 | s_callbacks.ModelicaError(string); 55 | } else { 56 | printf(string); 57 | } 58 | } 59 | 60 | 61 | void ModelicaFormatError(const char *string, ...) { 62 | 63 | va_list vl; 64 | va_start(vl, string); 65 | 66 | if (s_callbacks.ModelicaVFormatError) { 67 | s_callbacks.ModelicaVFormatError(string, vl); 68 | } else { 69 | vprintf(string, vl); 70 | exit(1); 71 | } 72 | 73 | va_end(vl); 74 | } 75 | 76 | 77 | void ModelicaVFormatError(const char *string, va_list vl) { 78 | 79 | if (s_callbacks.ModelicaVFormatError) { 80 | s_callbacks.ModelicaVFormatError(string, vl); 81 | } else { 82 | vprintf(string, vl); 83 | exit(1); 84 | } 85 | } 86 | 87 | 88 | char* ModelicaAllocateString(size_t len) { 89 | return static_cast(malloc(len)); 90 | } 91 | 92 | 93 | char* ModelicaAllocateStringWithErrorReturn(size_t len) { 94 | return static_cast(malloc(len)); 95 | } 96 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/ModelicaUtilities.h: -------------------------------------------------------------------------------- 1 | #ifndef MODELICA_UTILITIES_H 2 | #define MODELICA_UTILITIES_H 3 | 4 | #include 5 | #include 6 | #if defined(__cplusplus) 7 | extern "C" { 8 | #endif 9 | 10 | /* Utility functions which can be called by external Modelica functions. 11 | 12 | These functions are defined in section 12.8.6 of the 13 | Modelica Specification 3.0 and section 12.9.6 of the 14 | Modelica Specification 3.1 and 3.2. 15 | 16 | A generic C-implementation of these functions cannot be given, 17 | because it is tool dependent how strings are output in a 18 | window of the respective simulation tool. Therefore, only 19 | this header file is shipped with the Modelica Standard Library. 20 | */ 21 | 22 | /* 23 | * Some of the functions never return to the caller. In order to compile 24 | * external Modelica C-code in most compilers, noreturn attributes need to 25 | * be present to avoid warnings or errors. 26 | * 27 | * The following macros handle noreturn attributes according to the latest 28 | * C11/C++11 standard with fallback to GNU or MSVC extensions if using an 29 | * older compiler. 30 | */ 31 | #if __STDC_VERSION__ >= 201112L 32 | #define MODELICA_NORETURN _Noreturn 33 | #define MODELICA_NORETURNATTR 34 | #elif __cplusplus >= 201103L 35 | #define MODELICA_NORETURN [[noreturn]] 36 | #define MODELICA_NORETURNATTR 37 | #elif defined(__GNUC__) 38 | #define MODELICA_NORETURN 39 | #define MODELICA_NORETURNATTR __attribute__((noreturn)) 40 | #elif defined(_MSC_VER) 41 | #define MODELICA_NORETURN __declspec(noreturn) 42 | #define MODELICA_NORETURNATTR 43 | #else 44 | #define MODELICA_NORETURN 45 | #define MODELICA_NORETURNATTR 46 | #endif 47 | 48 | void ModelicaMessage(const char *string); 49 | /* 50 | Output the message string (no format control). 51 | */ 52 | 53 | 54 | void ModelicaFormatMessage(const char *string,...); 55 | /* 56 | Output the message under the same format control as the C-function printf. 57 | */ 58 | 59 | 60 | void ModelicaVFormatMessage(const char *string, va_list); 61 | /* 62 | Output the message under the same format control as the C-function vprintf. 63 | */ 64 | 65 | 66 | MODELICA_NORETURN void ModelicaError(const char *string) MODELICA_NORETURNATTR; 67 | /* 68 | Output the error message string (no format control). This function 69 | never returns to the calling function, but handles the error 70 | similarly to an assert in the Modelica code. 71 | */ 72 | 73 | 74 | MODELICA_NORETURN void ModelicaFormatError(const char *string,...) MODELICA_NORETURNATTR; 75 | /* 76 | Output the error message under the same format control as the C-function 77 | printf. This function never returns to the calling function, 78 | but handles the error similarly to an assert in the Modelica code. 79 | */ 80 | 81 | 82 | MODELICA_NORETURN void ModelicaVFormatError(const char *string, va_list) MODELICA_NORETURNATTR; 83 | /* 84 | Output the error message under the same format control as the C-function 85 | vprintf. This function never returns to the calling function, 86 | but handles the error similarly to an assert in the Modelica code. 87 | */ 88 | 89 | 90 | char* ModelicaAllocateString(size_t len); 91 | /* 92 | Allocate memory for a Modelica string which is used as return 93 | argument of an external Modelica function. Note, that the storage 94 | for string arrays (= pointer to string array) is still provided by the 95 | calling program, as for any other array. If an error occurs, this 96 | function does not return, but calls "ModelicaError". 97 | */ 98 | 99 | 100 | char* ModelicaAllocateStringWithErrorReturn(size_t len); 101 | /* 102 | Same as ModelicaAllocateString, except that in case of error, the 103 | function returns 0. This allows the external function to close files 104 | and free other open resources in case of error. After cleaning up 105 | resources use ModelicaError or ModelicaFormatError to signal 106 | the error. 107 | */ 108 | 109 | #if defined(__cplusplus) 110 | } 111 | #endif 112 | 113 | #endif -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/PID_v1.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_v1_h 2 | #define PID_v1_h 3 | #define LIBRARY_VERSION 1.1.1 4 | 5 | class PID 6 | { 7 | 8 | 9 | public: 10 | 11 | //Constants used in some of the functions below 12 | #define AUTOMATIC 1 13 | #define MANUAL 0 14 | #define DIRECT 0 15 | #define REVERSE 1 16 | 17 | //commonly used functions ************************************************************************** 18 | PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and 19 | double, double, double, int); // Setpoint. Initial tuning parameters are also set here 20 | 21 | void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) 22 | 23 | bool Compute(); // * performs the PID calculation. it should be 24 | // called every time loop() cycles. ON/OFF and 25 | // calculation frequency can be set using SetMode 26 | // SetSampleTime respectively 27 | 28 | void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but 29 | //it's likely the user will want to change this depending on 30 | //the application 31 | 32 | 33 | 34 | //available but not commonly used functions ******************************************************** 35 | void SetTunings(double, double, // * While most users will set the tunings once in the 36 | double); // constructor, this function gives the user the option 37 | // of changing tunings during runtime for Adaptive control 38 | void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT 39 | // means the output will increase when error is positive. REVERSE 40 | // means the opposite. it's very unlikely that this will be needed 41 | // once it is set in the constructor. 42 | void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which 43 | // the PID calculation is performed. default is 100 44 | 45 | 46 | 47 | //Display functions **************************************************************** 48 | double GetKp(); // These functions query the pid for interal values. 49 | double GetKi(); // they were created mainly for the pid front-end, 50 | double GetKd(); // where it's important to know what is actually 51 | int GetMode(); // inside the PID. 52 | int GetDirection(); // 53 | 54 | private: 55 | void Initialize(); 56 | 57 | double dispKp; // * we'll hold on to the tuning parameters in user-entered 58 | double dispKi; // format for display purposes 59 | double dispKd; // 60 | 61 | double kp; // * (P)roportional Tuning Parameter 62 | double ki; // * (I)ntegral Tuning Parameter 63 | double kd; // * (D)erivative Tuning Parameter 64 | 65 | int controllerDirection; 66 | 67 | double *myInput; // * Pointers to the Input, Output, and Setpoint variables 68 | double *myOutput; // This creates a hard link between the variables and the 69 | double *mySetpoint; // PID, freeing the user from having to constantly tell us 70 | // what these values are. with pointers we'll just know. 71 | 72 | unsigned long lastTime; 73 | double ITerm, lastInput; 74 | 75 | unsigned long SampleTime; 76 | double outMin, outMax; 77 | bool inAuto; 78 | }; 79 | #endif 80 | 81 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/Print.cpp: -------------------------------------------------------------------------------- 1 | #include "Print.h" 2 | 3 | #include // for isnan(), isinf() 4 | 5 | #ifndef isnan 6 | #define isnan(x) _isnan(x) 7 | #endif 8 | 9 | #ifndef isinf 10 | #define isinf(x) (!_finite(x)) 11 | #endif 12 | 13 | /* default implementation: may be overridden */ 14 | size_t Print::write(const uint8_t *buffer, size_t size) 15 | { 16 | size_t n = 0; 17 | while (size--) { 18 | if (write(*buffer++)) n++; 19 | else break; 20 | } 21 | return n; 22 | } 23 | 24 | size_t Print::print(const char str[]) 25 | { 26 | return write(str); 27 | } 28 | 29 | size_t Print::print(char c) 30 | { 31 | return write(c); 32 | } 33 | 34 | size_t Print::print(unsigned char b, int base) 35 | { 36 | return print((unsigned long) b, base); 37 | } 38 | 39 | size_t Print::print(int n, int base) 40 | { 41 | return print((long) n, base); 42 | } 43 | 44 | size_t Print::print(unsigned int n, int base) 45 | { 46 | return print((unsigned long) n, base); 47 | } 48 | 49 | size_t Print::print(long n, int base) 50 | { 51 | if (base == 0) { 52 | return write(n); 53 | } else if (base == 10) { 54 | if (n < 0) { 55 | int t = print('-'); 56 | n = -n; 57 | return printNumber(n, 10) + t; 58 | } 59 | return printNumber(n, 10); 60 | } else { 61 | return printNumber(n, base); 62 | } 63 | } 64 | 65 | size_t Print::print(unsigned long n, int base) 66 | { 67 | if (base == 0) return write(n); 68 | else return printNumber(n, base); 69 | } 70 | 71 | size_t Print::print(double n, int digits) 72 | { 73 | return printFloat(n, digits); 74 | } 75 | 76 | size_t Print::println(void) 77 | { 78 | return write("\n"); 79 | } 80 | 81 | size_t Print::println(const char c[]) 82 | { 83 | size_t n = print(c); 84 | n += println(); 85 | return n; 86 | } 87 | 88 | size_t Print::println(char c) 89 | { 90 | size_t n = print(c); 91 | n += println(); 92 | return n; 93 | } 94 | 95 | size_t Print::println(unsigned char b, int base) 96 | { 97 | size_t n = print(b, base); 98 | n += println(); 99 | return n; 100 | } 101 | 102 | size_t Print::println(int num, int base) 103 | { 104 | size_t n = print(num, base); 105 | n += println(); 106 | return n; 107 | } 108 | 109 | size_t Print::println(unsigned int num, int base) 110 | { 111 | size_t n = print(num, base); 112 | n += println(); 113 | return n; 114 | } 115 | 116 | size_t Print::println(long num, int base) 117 | { 118 | size_t n = print(num, base); 119 | n += println(); 120 | return n; 121 | } 122 | 123 | size_t Print::println(unsigned long num, int base) 124 | { 125 | size_t n = print(num, base); 126 | n += println(); 127 | return n; 128 | } 129 | 130 | size_t Print::println(double num, int digits) 131 | { 132 | size_t n = print(num, digits); 133 | n += println(); 134 | return n; 135 | } 136 | 137 | // Private Methods ///////////////////////////////////////////////////////////// 138 | 139 | size_t Print::printNumber(unsigned long n, uint8_t base) 140 | { 141 | char buf[8 * sizeof(long) + 1]; // Assumes 8-bit chars plus zero byte. 142 | char *str = &buf[sizeof(buf) - 1]; 143 | 144 | *str = '\0'; 145 | 146 | // prevent crash if called with base == 1 147 | if (base < 2) base = 10; 148 | 149 | do { 150 | char c = n % base; 151 | n /= base; 152 | 153 | *--str = c < 10 ? c + '0' : c + 'A' - 10; 154 | } while(n); 155 | 156 | return write(str); 157 | } 158 | 159 | size_t Print::printFloat(double number, uint8_t digits) 160 | { 161 | size_t n = 0; 162 | 163 | if (isnan(number)) return print("nan"); 164 | if (isinf(number)) return print("inf"); 165 | if (number > 4294967040.0) return print ("ovf"); // constant determined empirically 166 | if (number <-4294967040.0) return print ("ovf"); // constant determined empirically 167 | 168 | // Handle negative numbers 169 | if (number < 0.0) 170 | { 171 | n += print('-'); 172 | number = -number; 173 | } 174 | 175 | // Round correctly so that print(1.999, 2) prints as "2.00" 176 | double rounding = 0.5; 177 | for (uint8_t i=0; i 0) { 189 | n += print("."); 190 | } 191 | 192 | // Extract digits from the remainder one at a time 193 | while (digits-- > 0) 194 | { 195 | remainder *= 10.0; 196 | unsigned int toPrint = (unsigned int)(remainder); 197 | n += print(toPrint); 198 | remainder -= toPrint; 199 | } 200 | 201 | return n; 202 | } 203 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/Print.h: -------------------------------------------------------------------------------- 1 | /* 2 | Print.h - Base class that provides print() and println() 3 | Copyright (c) 2008 David A. Mellis. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #ifndef Print_h 21 | #define Print_h 22 | 23 | #include "Arduino.h" 24 | 25 | //#include 26 | #include // for size_t 27 | 28 | //#include "WString.h" 29 | //#include "Printable.h" 30 | 31 | #define DEC 10 32 | #define HEX 16 33 | #define OCT 8 34 | #define BIN 2 35 | 36 | class Print { 37 | 38 | private: 39 | int write_error; 40 | size_t printNumber(unsigned long, uint8_t); 41 | size_t printFloat(double, uint8_t); 42 | 43 | protected: 44 | void setWriteError(int err = 1) { write_error = err; } 45 | 46 | public: 47 | Print() : write_error(0) {} 48 | 49 | int getWriteError() { return write_error; } 50 | void clearWriteError() { setWriteError(0); } 51 | 52 | virtual size_t write(uint8_t) = 0; 53 | size_t write(const char *str) { 54 | if (str == NULL) return 0; 55 | return write((const uint8_t *)str, strlen(str)); 56 | } 57 | virtual size_t write(const uint8_t *buffer, size_t size); 58 | size_t write(const char *buffer, size_t size) { 59 | return write((const uint8_t *)buffer, size); 60 | } 61 | 62 | // size_t print(const __FlashStringHelper *); 63 | // size_t print(const String &); 64 | size_t print(const char[]); 65 | size_t print(char); 66 | size_t print(unsigned char, int = DEC); 67 | size_t print(int, int = DEC); 68 | size_t print(unsigned int, int = DEC); 69 | size_t print(long, int = DEC); 70 | size_t print(unsigned long, int = DEC); 71 | size_t print(double, int = 2); 72 | // size_t print(const Printable&); 73 | 74 | // size_t println(const __FlashStringHelper *); 75 | // size_t println(const String &s); 76 | size_t println(const char[]); 77 | size_t println(char); 78 | size_t println(unsigned char, int = DEC); 79 | size_t println(int, int = DEC); 80 | size_t println(unsigned int, int = DEC); 81 | size_t println(long, int = DEC); 82 | size_t println(unsigned long, int = DEC); 83 | size_t println(double, int = 2); 84 | // size_t println(const Printable&); 85 | size_t println(void); 86 | }; 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/Servo.cpp: -------------------------------------------------------------------------------- 1 | #include "Servo.h" 2 | #include "SoftArduino.h" 3 | #include "ModelicaUtilities.h" 4 | 5 | 6 | uint8_t Servo::attach(int pin) { 7 | 8 | //ModelicaFormatMessage("Servo::attach(%d)\n", pin); 9 | 10 | // TODO: Check pin >= 0 && pin < NUM_PINS 11 | 12 | m_pin = pin; 13 | 14 | INSTANCE->portMode[pin] = SoftArduino::PORT_MODE_PWM; 15 | INSTANCE->pulsePeriod[pin] = REFRESH_INTERVAL; 16 | 17 | return 1; 18 | } 19 | 20 | uint8_t Servo::attach(int pin, int min, int max) { 21 | 22 | // TODO: check min < max 23 | 24 | m_minPulseWidth = min; 25 | m_maxPulseWidth = max; 26 | 27 | return attach(pin); 28 | } 29 | 30 | void Servo::detach() { 31 | 32 | INSTANCE->portMode[m_pin] = SoftArduino::PORT_MODE_DIGITAL; 33 | INSTANCE->pulsePeriod[m_pin] = SoftArduino::DEFAULT_PULSE_PERIOD; 34 | 35 | m_pin = -1; 36 | } 37 | 38 | void Servo::write(int value) { 39 | 40 | //ModelicaFormatMessage("Servo::write(%d)\n", value); 41 | 42 | if (value < 200) { 43 | int pulseWidth = static_cast(m_minPulseWidth + (m_maxPulseWidth - m_minPulseWidth) * (value / 180.)); 44 | writeMicroseconds(pulseWidth); 45 | } else { 46 | writeMicroseconds(value); 47 | } 48 | 49 | } 50 | 51 | void Servo::writeMicroseconds(int value) { 52 | 53 | if (m_pin < 0 || m_pin >= NUM_DIGITAL_PINS) { 54 | ModelicaFormatError("Failed to write microseconds. Illegal pin: %d\n", m_pin); 55 | return; 56 | } 57 | 58 | m_pulseWidth = value; 59 | 60 | INSTANCE->pulseWidth[m_pin] = m_pulseWidth; 61 | } 62 | 63 | int Servo::read() { 64 | // calculate the angle from the current pulse width 65 | return int(180. * (m_pulseWidth - m_minPulseWidth) / (m_maxPulseWidth - m_minPulseWidth)); 66 | } 67 | 68 | int Servo::readMicroseconds() { 69 | return m_pulseWidth; 70 | } 71 | 72 | bool Servo::attached() { 73 | return m_pin >= 0; 74 | } 75 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/Servo.h: -------------------------------------------------------------------------------- 1 | /* 2 | Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 3 | Copyright (c) 2009 Michael Margolis. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | /* 21 | A servo is activated by creating an instance of the Servo class passing 22 | the desired pin to the attach() method. 23 | The servos are pulsed in the background using the value most recently 24 | written using the write() method. 25 | 26 | Note that analogWrite of PWM on pins associated with the timer are 27 | disabled when the first servo is attached. 28 | Timers are seized as needed in groups of 12 servos - 24 servos use two 29 | timers, 48 servos will use four. 30 | The sequence used to sieze timers is defined in timers.h 31 | 32 | The methods are: 33 | 34 | Servo - Class for manipulating servo motors connected to Arduino pins. 35 | 36 | attach(pin ) - Attaches a servo motor to an i/o pin. 37 | attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds 38 | default min is 544, max is 2400 39 | 40 | write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds) 41 | writeMicroseconds() - Sets the servo pulse width in microseconds 42 | read() - Gets the last written servo pulse width as an angle between 0 and 180. 43 | readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release) 44 | attached() - Returns true if there is a servo attached. 45 | detach() - Stops an attached servos from pulsing its i/o pin. 46 | */ 47 | 48 | #ifndef Servo_h 49 | #define Servo_h 50 | 51 | #ifdef _MSC_VER 52 | #include 53 | #else 54 | #include 55 | #endif 56 | 57 | 58 | #define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo 59 | #define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo 60 | #define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached 61 | #define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds 62 | 63 | class Servo 64 | { 65 | public: 66 | // Servo(); 67 | uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure 68 | uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes. 69 | void detach(); 70 | void write(int value); // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds 71 | void writeMicroseconds(int value); // Write pulse width in microseconds 72 | int read(); // returns current pulse width as an angle between 0 and 180 degrees 73 | int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release) 74 | bool attached(); // return true if this servo is attached, otherwise false 75 | private: 76 | int m_pin = -1; // index of the digital pin 77 | int m_pulseWidth = DEFAULT_PULSE_WIDTH; // current pulse width in microseconds 78 | int m_minPulseWidth = MIN_PULSE_WIDTH; // mininum pulse witdh in microseconds 79 | int m_maxPulseWidth = MAX_PULSE_WIDTH; // maximum pulse witdh in microseconds 80 | //uint8_t servoIndex; // index into the channel data for this servo 81 | // int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH 82 | // int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH 83 | }; 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/Sketch.cpp: -------------------------------------------------------------------------------- 1 | #include "Sketch.h" 2 | 3 | #include "Arduino.h" 4 | #include "SoftSerial.h" 5 | 6 | SoftSerial Serial; 7 | 8 | // include your sketch here 9 | #include "Blink.ino" 10 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/Sketch.h: -------------------------------------------------------------------------------- 1 | #ifndef SKETCH_H 2 | #define SKETCH_H 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | void setup(); 9 | 10 | void loop(); 11 | 12 | #ifdef __cplusplus 13 | } // extern "C" 14 | #endif 15 | 16 | #endif // SKETCH_H 17 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/SoftArduino.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "Event.h" 5 | 6 | #include "Arduino.h" 7 | 8 | typedef void (*InterruptSeriveRoutine)(); 9 | 10 | #define interruptToDigitalPin(i) ((i) == 0 ? 2 : ((i) == 1 ? 3 : NOT_A_PIN)) 11 | 12 | #define INSTANCE SoftArduino::getInstance() 13 | 14 | 15 | class SoftInterrupt { 16 | 17 | public: 18 | InterruptSeriveRoutine isr; 19 | int mode; // CHANGE = 1, FALLING = 2 or RISING = 3 20 | int prePotential; // LOW or HIGH 21 | 22 | explicit SoftInterrupt(InterruptSeriveRoutine isr, int mode, int potential) : isr(isr), mode(mode), prePotential(potential) {} 23 | 24 | void update(int potential); 25 | 26 | }; 27 | 28 | 29 | class SoftArduino { 30 | 31 | public: 32 | static const int PORT_MODE_INPUT = 0; 33 | static const int PORT_MODE_DIGITAL = 1; 34 | static const int PORT_MODE_PWM = 2; 35 | static const int DEFAULT_PULSE_PERIOD = 2000; // default pulse period in microseconds 36 | 37 | unsigned long time = 0; // current time in micro seconds 38 | int analogReferenceMode = DEFAULT; // analog reference mode [DEFAULT, INTERNAL, INTERNAL1V1 or EXTERNAL] 39 | double potential[NUM_DIGITAL_PINS] = {}; // potential of the pins normalized to the reference voltage 40 | int portMode[NUM_DIGITAL_PINS] = {}; // 0 = input, 1 = digital, 2 = PWM 41 | int pulseWidth[NUM_DIGITAL_PINS] = {}; // pulse width in microseconds 42 | int pulsePeriod[NUM_DIGITAL_PINS] = { DEFAULT_PULSE_PERIOD }; // pulse period in microseconds 43 | 44 | bool interruptsEnabled = true; 45 | 46 | SoftInterrupt *interrupts[2] = {}; 47 | 48 | const char *error = nullptr; 49 | 50 | void update(); 51 | 52 | void synchronize(); 53 | 54 | static SoftArduino* getInstance() { 55 | static SoftArduino instance; 56 | return &instance; 57 | } 58 | 59 | std::thread thread; 60 | std::atomic terminate; 61 | Event inputReady; 62 | Event outputReady; 63 | 64 | private: 65 | explicit SoftArduino(); 66 | ~SoftArduino(); 67 | 68 | static void runSketch(); 69 | 70 | }; 71 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/SoftSerial.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Print.h" 4 | #include "ModelicaUtilities.h" 5 | 6 | 7 | class SoftSerial : public Print { 8 | 9 | public: 10 | 11 | void begin(unsigned long baud) { } 12 | 13 | size_t write(uint8_t c) override { 14 | return write(&c, 1); 15 | } 16 | 17 | size_t write(const uint8_t *buffer, size_t size) override { 18 | ModelicaMessage(reinterpret_cast(buffer)); 19 | return size; 20 | } 21 | 22 | }; 23 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/WCharacter.h: -------------------------------------------------------------------------------- 1 | /* 2 | WCharacter.h - Character utility functions for Wiring & Arduino 3 | Copyright (c) 2010 Hernando Barragan. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #ifndef Character_h 21 | #define Character_h 22 | 23 | #include 24 | 25 | #define __attribute__(x) 26 | #define boolean bool 27 | 28 | // WCharacter.h prototypes 29 | inline boolean isAlphaNumeric(int c) __attribute__((always_inline)); 30 | inline boolean isAlpha(int c) __attribute__((always_inline)); 31 | inline boolean isAscii(int c) __attribute__((always_inline)); 32 | inline boolean isWhitespace(int c) __attribute__((always_inline)); 33 | inline boolean isControl(int c) __attribute__((always_inline)); 34 | inline boolean isDigit(int c) __attribute__((always_inline)); 35 | inline boolean isGraph(int c) __attribute__((always_inline)); 36 | inline boolean isLowerCase(int c) __attribute__((always_inline)); 37 | inline boolean isPrintable(int c) __attribute__((always_inline)); 38 | inline boolean isPunct(int c) __attribute__((always_inline)); 39 | inline boolean isSpace(int c) __attribute__((always_inline)); 40 | inline boolean isUpperCase(int c) __attribute__((always_inline)); 41 | inline boolean isHexadecimalDigit(int c) __attribute__((always_inline)); 42 | inline int toAscii(int c) __attribute__((always_inline)); 43 | inline int toLowerCase(int c) __attribute__((always_inline)); 44 | inline int toUpperCase(int c)__attribute__((always_inline)); 45 | 46 | 47 | // Checks for an alphanumeric character. 48 | // It is equivalent to (isalpha(c) || isdigit(c)). 49 | inline boolean isAlphaNumeric(int c) 50 | { 51 | return ( isalnum(c) == 0 ? false : true); 52 | } 53 | 54 | 55 | // Checks for an alphabetic character. 56 | // It is equivalent to (isupper(c) || islower(c)). 57 | inline boolean isAlpha(int c) 58 | { 59 | return ( isalpha(c) == 0 ? false : true); 60 | } 61 | 62 | 63 | // Checks whether c is a 7-bit unsigned char value 64 | // that fits into the ASCII character set. 65 | inline boolean isAscii(int c) 66 | { 67 | return ( isascii (c) == 0 ? false : true); 68 | } 69 | 70 | 71 | // Checks for a blank character, that is, a space or a tab. 72 | inline boolean isWhitespace(int c) 73 | { 74 | // TODO 75 | return false; //( isblank (c) == 0 ? false : true); 76 | } 77 | 78 | 79 | // Checks for a control character. 80 | inline boolean isControl(int c) 81 | { 82 | return ( iscntrl (c) == 0 ? false : true); 83 | } 84 | 85 | 86 | // Checks for a digit (0 through 9). 87 | inline boolean isDigit(int c) 88 | { 89 | return ( isdigit (c) == 0 ? false : true); 90 | } 91 | 92 | 93 | // Checks for any printable character except space. 94 | inline boolean isGraph(int c) 95 | { 96 | return ( isgraph (c) == 0 ? false : true); 97 | } 98 | 99 | 100 | // Checks for a lower-case character. 101 | inline boolean isLowerCase(int c) 102 | { 103 | return (islower (c) == 0 ? false : true); 104 | } 105 | 106 | 107 | // Checks for any printable character including space. 108 | inline boolean isPrintable(int c) 109 | { 110 | return ( isprint (c) == 0 ? false : true); 111 | } 112 | 113 | 114 | // Checks for any printable character which is not a space 115 | // or an alphanumeric character. 116 | inline boolean isPunct(int c) 117 | { 118 | return ( ispunct (c) == 0 ? false : true); 119 | } 120 | 121 | 122 | // Checks for white-space characters. For the avr-libc library, 123 | // these are: space, formfeed ('\f'), newline ('\n'), carriage 124 | // return ('\r'), horizontal tab ('\t'), and vertical tab ('\v'). 125 | inline boolean isSpace(int c) 126 | { 127 | return ( isspace (c) == 0 ? false : true); 128 | } 129 | 130 | 131 | // Checks for an uppercase letter. 132 | inline boolean isUpperCase(int c) 133 | { 134 | return ( isupper (c) == 0 ? false : true); 135 | } 136 | 137 | 138 | // Checks for a hexadecimal digits, i.e. one of 0 1 2 3 4 5 6 7 139 | // 8 9 a b c d e f A B C D E F. 140 | inline boolean isHexadecimalDigit(int c) 141 | { 142 | return ( isxdigit (c) == 0 ? false : true); 143 | } 144 | 145 | 146 | // Converts c to a 7-bit unsigned char value that fits into the 147 | // ASCII character set, by clearing the high-order bits. 148 | inline int toAscii(int c) 149 | { 150 | return toascii (c); 151 | } 152 | 153 | 154 | // Warning: 155 | // Many people will be unhappy if you use this function. 156 | // This function will convert accented letters into random 157 | // characters. 158 | 159 | // Converts the letter c to lower case, if possible. 160 | inline int toLowerCase(int c) 161 | { 162 | return tolower (c); 163 | } 164 | 165 | 166 | // Converts the letter c to upper case, if possible. 167 | inline int toUpperCase(int c) 168 | { 169 | return toupper (c); 170 | } 171 | 172 | #endif 173 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/WMath.cpp: -------------------------------------------------------------------------------- 1 | /* -*- mode: jde; c-basic-offset: 2; indent-tabs-mode: nil -*- */ 2 | 3 | /* 4 | Part of the Wiring project - http://wiring.org.co 5 | Copyright (c) 2004-06 Hernando Barragan 6 | Modified 13 August 2006, David A. Mellis for Arduino - http://www.arduino.cc/ 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version. 12 | 13 | This library is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General 19 | Public License along with this library; if not, write to the 20 | Free Software Foundation, Inc., 59 Temple Place, Suite 330, 21 | Boston, MA 02111-1307 USA 22 | */ 23 | 24 | 25 | 26 | extern "C" { 27 | #include "stdlib.h" 28 | } 29 | 30 | void randomSeed(unsigned long seed) 31 | { 32 | if (seed != 0) { 33 | srand(seed); 34 | } 35 | } 36 | 37 | long random(long howbig) 38 | { 39 | if (howbig == 0) { 40 | return 0; 41 | } 42 | return rand() % howbig; 43 | } 44 | 45 | long random(long howsmall, long howbig) 46 | { 47 | if (howsmall >= howbig) { 48 | return howsmall; 49 | } 50 | long diff = howbig - howsmall; 51 | return random(diff) + howsmall; 52 | } 53 | 54 | long map(long x, long in_min, long in_max, long out_min, long out_max) 55 | { 56 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 57 | } 58 | 59 | unsigned int makeWord(unsigned int w) { return w; } 60 | unsigned int makeWord(unsigned char h, unsigned char l) { return (h << 8) | l; } 61 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Arduino/stdbool.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 1998, 1999, 2000, 2009 Free Software Foundation, Inc. 2 | 3 | This file is part of GCC. 4 | 5 | GCC is free software; you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation; either version 3, or (at your option) 8 | any later version. 9 | 10 | GCC is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | Under Section 7 of GPL version 3, you are granted additional 16 | permissions described in the GCC Runtime Library Exception, version 17 | 3.1, as published by the Free Software Foundation. 18 | 19 | You should have received a copy of the GNU General Public License and 20 | a copy of the GCC Runtime Library Exception along with this program; 21 | see the files COPYING3 and COPYING.RUNTIME respectively. If not, see 22 | . */ 23 | 24 | /* 25 | * ISO C Standard: 7.16 Boolean type and values 26 | */ 27 | 28 | #ifndef _STDBOOL_H 29 | #define _STDBOOL_H 30 | 31 | #ifndef __cplusplus 32 | 33 | #define bool _Bool 34 | #define true 1 35 | #define false 0 36 | 37 | #else /* __cplusplus */ 38 | 39 | /* Supporting in C++ is a GCC extension. */ 40 | #define _Bool bool 41 | #define bool bool 42 | #define false false 43 | #define true true 44 | 45 | #endif /* __cplusplus */ 46 | 47 | /* Signal that all the definitions are present. */ 48 | #define __bool_true_false_are_defined 1 49 | 50 | #endif /* stdbool.h */ 51 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Firmata/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 3.15) 2 | 3 | set(CMAKE_MSVC_RUNTIME_LIBRARY "MultiThreaded$<$:Debug>") 4 | 5 | project (ModelicaFirmata) 6 | 7 | if ("${CMAKE_SIZEOF_VOID_P}" STREQUAL "8") 8 | set(PLATFORM win64) 9 | else () 10 | set(PLATFORM win32) 11 | endif () 12 | 13 | add_library(ModelicaFirmata SHARED 14 | FirmataConnection.cpp 15 | FirmataConnection.h 16 | ModelicaFirmata.cpp 17 | ModelicaFirmata.h 18 | serial.cpp 19 | serial.h 20 | ../../Include/ModelicaUtilityFunctions.c 21 | ../Arduino/ModelicaUtilities.cpp 22 | ) 23 | 24 | target_compile_definitions(ModelicaFirmata PUBLIC 25 | WINDOWS 26 | _CRT_SECURE_NO_WARNINGS 27 | ) 28 | 29 | 30 | #set_property(TARGET ModelicaFirmata PROPERTY 31 | # MSVC_RUNTIME_LIBRARY "MultiThreaded$<$:Debug>") 32 | 33 | set_target_properties(ModelicaFirmata PROPERTIES PREFIX "") 34 | 35 | target_include_directories(ModelicaFirmata PUBLIC 36 | . 37 | ../../Include 38 | ../../Sketches 39 | ../Arduino 40 | ) 41 | 42 | add_custom_command(TARGET ModelicaFirmata POST_BUILD COMMAND ${CMAKE_COMMAND} -E copy 43 | "$" 44 | "${CMAKE_CURRENT_SOURCE_DIR}/../../Library/${PLATFORM}" 45 | ) 46 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Firmata/FirmataConnection.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "serial.h" 8 | 9 | #include "ModelicaUtilityFunctions.h" 10 | 11 | 12 | typedef struct { 13 | uint8_t mode; 14 | uint8_t analog_channel; 15 | uint64_t supported_modes; 16 | uint32_t value; 17 | uint32_t next_value; 18 | uint8_t ready; 19 | } pin_t; 20 | 21 | 22 | class FirmataConnection { 23 | 24 | public: 25 | typedef void vFormatMessageTYPE(const char *, va_list); 26 | typedef void vFormatErrorTYPE(const char *, va_list); 27 | 28 | explicit FirmataConnection( 29 | std::string port, 30 | bool showCapabilitites = false, 31 | int samplingInterval = 10, 32 | int baudRate = 57600, 33 | vFormatMessageTYPE *vFormatMessage = nullptr, 34 | vFormatErrorTYPE *vFormatError = nullptr); 35 | 36 | std::string getPortName(); 37 | int openPort(); 38 | void closePort(); 39 | std::vector getPortList(); 40 | void setPinMode(uint32_t pin, uint32_t mode); 41 | void writeDigitalPin(uint32_t pin, uint32_t value); 42 | void writeAnalogPin(uint32_t pin, uint32_t value); 43 | void writeServoPin(uint32_t pin, uint32_t value, int min, int max); 44 | double readAnalogPin(uint32_t pin, double min, double max, double init); 45 | uint32_t readDigitalPin(uint32_t pin, int init); 46 | void setServoConfig(uint32_t pin, uint32_t min, uint32_t max); 47 | 48 | public: 49 | static FirmataConnection *getBoard(uint32_t id); 50 | static int getPortIndex(FirmataConnection* elem); 51 | 52 | void initialize(bool dtr); 53 | void updateBoard(int timeout); 54 | void reportFirmware(); 55 | void showPinCapabilities(); 56 | void setSamplingInterval(); 57 | void setAsFakePort(); 58 | 59 | inline int numSentMessages() { return m_sent; } 60 | inline int numReceivedMessages() { return m_received; } 61 | 62 | private: 63 | int m_sent = 0; 64 | int m_received = 0; 65 | 66 | static std::vector m_ports; 67 | 68 | int write(const void *ptr, int len); 69 | int read(void *ptr, int count, int timeout); 70 | void Parse(const uint8_t *buf, int len); 71 | void DoMessage(void); 72 | 73 | // logging functions 74 | void info(const char *format, ...); 75 | void error(const char *format, ...); 76 | 77 | pin_t m_pinInfo[128]; 78 | std::string m_portName; 79 | bool m_showCapabilities; 80 | int m_samplingInterval; 81 | int m_baudRate; 82 | Serial m_serial; 83 | bool m_ready = false; 84 | 85 | int m_parseCount = -1; 86 | int m_parseCommandLen = -1; 87 | uint8_t m_parseBuf[4096]; 88 | 89 | // callbacks 90 | vFormatMessageTYPE *m_vFormatMessage = nullptr; 91 | vFormatErrorTYPE *m_vFormatError = nullptr; 92 | }; 93 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Firmata/ModelicaFirmata.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Modelica interface functions for the ExternalFirmata object 3 | * 4 | * Copyright 2017 Dassault Systemes. Dveloped by Torsten Sommer (torsten.sommer@3ds.com). 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #include "ModelicaFirmata.h" 21 | #include "FirmataConnection.h" 22 | 23 | #include "ModelicaUtilityFunctions.h" 24 | 25 | 26 | /** Clamps value between 0 and 1 and returns value. */ 27 | inline double clamp01(double value) { 28 | return value > 1.0 ? 1.0 : (value < 0.0 ? 0.0 : value); 29 | } 30 | 31 | 32 | void *createFirmataConnection(const char *port, int showCapabilitites, int samplingMs, int baudRate, int dtr, void *callbacks) { 33 | 34 | auto cb = reinterpret_cast(callbacks); 35 | setModelicaUtilityFunctions(cb); 36 | return new FirmataConnection(port, showCapabilitites, samplingMs, baudRate, cb->ModelicaVFormatMessage, cb->ModelicaVFormatError); 37 | } 38 | 39 | 40 | void freeFirmataConnection(void *object) { 41 | 42 | if (object) { 43 | auto board = reinterpret_cast(object); 44 | board->closePort(); 45 | delete board; 46 | } 47 | } 48 | 49 | 50 | void updateBoard(int id) { 51 | auto board = FirmataConnection::getBoard(id); 52 | board->updateBoard(0); 53 | } 54 | 55 | 56 | int getBoardId(void *object) { 57 | 58 | if (object) { 59 | return FirmataConnection::getPortIndex(reinterpret_cast(object)); 60 | } 61 | 62 | return -1; 63 | } 64 | 65 | 66 | double readAnalogPin(int pin, double min, double max, double init, int id) { 67 | auto board = FirmataConnection::getBoard(id); 68 | return board->readAnalogPin(pin, min, max, init); 69 | } 70 | 71 | 72 | int readDigitalPin(int pin, int init, int id) { 73 | auto board = FirmataConnection::getBoard(id); 74 | return board->readDigitalPin(pin, init); 75 | } 76 | 77 | 78 | void writeAnalogPin(int pin, int id, double value) { 79 | auto board = FirmataConnection::getBoard(id); 80 | board->writeAnalogPin(pin, uint32_t(clamp01(value) * 255)); 81 | } 82 | 83 | 84 | void writeDigitalPin(int pin, int id, int value) { 85 | auto board = FirmataConnection::getBoard(id); 86 | board->writeDigitalPin(pin, value); 87 | } 88 | 89 | 90 | void writeServoPin(int pin, int id, double value, int min, int max) { 91 | auto board = FirmataConnection::getBoard(id); 92 | board->writeServoPin(pin, int(clamp01(value) * 180), min, max); 93 | } 94 | 95 | int numSentMessages(int id) { 96 | auto board = FirmataConnection::getBoard(id); 97 | return board->numSentMessages(); 98 | } 99 | 100 | int numReceivedMessages(int id) { 101 | auto board = FirmataConnection::getBoard(id); 102 | return board->numReceivedMessages(); 103 | } 104 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Firmata/ModelicaFirmata.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifdef _MSC_VER 4 | #define EXPORT __declspec(dllexport) 5 | #else 6 | #define EXPORT __attribute__((visibility("default"))) 7 | #endif 8 | 9 | extern "C" { 10 | 11 | EXPORT void *createFirmataConnection(const char *port, int showCapabilitites, int samplingMs, int baudRate, int dtr, void *callbacks); 12 | EXPORT void freeFirmataConnection(void *object); 13 | 14 | EXPORT void updateBoard(int id); 15 | EXPORT int getBoardId(void *object); 16 | 17 | EXPORT double readAnalogPin (int pin, double min, double max, double init, int id); 18 | EXPORT int readDigitalPin (int pin, int init, int id); 19 | EXPORT void writeAnalogPin (int pin, int id, double value); 20 | EXPORT void writeDigitalPin(int pin, int id, int value); 21 | EXPORT void writeServoPin (int pin, int id, double value, int min, int max); 22 | 23 | EXPORT int numSentMessages(int id); 24 | EXPORT int numReceivedMessages(int id); 25 | 26 | } 27 | -------------------------------------------------------------------------------- /Arduino/Resources/Source/Firmata/serial.h: -------------------------------------------------------------------------------- 1 | #ifndef __serial_h__ 2 | #define __serial_h__ 3 | 4 | #include 5 | 6 | #if defined(LINUX) 7 | #include 8 | #elif defined(MACOSX) 9 | #include 10 | #elif defined(WINDOWS) 11 | #include 12 | #endif 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | using namespace std; 20 | 21 | class Serial 22 | { 23 | public: 24 | Serial(); 25 | ~Serial(); 26 | static vector port_list(); 27 | int Open(const string& name); 28 | string error_message(); 29 | int Set_baud(int baud); 30 | int Set_baud(const string& baud_str); 31 | int Read(void *ptr, int count); 32 | int Write(const void *ptr, int len); 33 | int Input_wait(int msec); 34 | void Input_discard(void); 35 | int Set_control(int dtr, int rts); 36 | void Output_flush(); 37 | void Close(void); 38 | int Is_open(void); 39 | string get_name(void); 40 | private: 41 | int port_is_open; 42 | string port_name; 43 | int baud_rate; 44 | string error_msg; 45 | private: 46 | #if defined(LINUX) || defined(MACOSX) 47 | int port_fd; 48 | struct termios settings_orig; 49 | struct termios settings; 50 | #elif defined(WINDOWS) 51 | HANDLE port_handle; 52 | COMMCONFIG port_cfg_orig; 53 | COMMCONFIG port_cfg; 54 | #endif 55 | }; 56 | 57 | #endif // __serial_h__ 58 | -------------------------------------------------------------------------------- /Arduino/package.mo: -------------------------------------------------------------------------------- 1 | within ; 2 | package Arduino "Library to simulate sketches on a virtual Arduino Uno and to connect models to real-world circuits using the Firmata protocol" 3 | 4 | extends Modelica.Icons.Package; 5 | 6 | 7 | 8 | 9 | 10 | annotation ( 11 | version="0.3.0", 12 | uses( 13 | Modelica(version="4.0.0"), Modelica_DeviceDrivers(version="2.0.0")), 14 | Icon(graphics={ 15 | Ellipse( 16 | extent={{-80,38},{4,-38}}, 17 | lineColor={0,135,143}, 18 | lineThickness=0.5), 19 | Ellipse( 20 | extent={{4,38},{88,-38}}, 21 | lineColor={0,135,143}, 22 | lineThickness=0.5), 23 | Line( 24 | points={{-56,0},{-24,0}}, 25 | color={0,135,143}, 26 | smooth=Smooth.Bezier), 27 | Line( 28 | points={{24,0},{68,0}}, 29 | color={0,135,143}, 30 | smooth=Smooth.Bezier), 31 | Line( 32 | points={{-20,-4.8986e-016},{12,0}}, 33 | color={0,135,143}, 34 | smooth=Smooth.Bezier, 35 | origin={46,4}, 36 | rotation=90)}), 37 | Documentation(info=" 38 |

For updates and tutorials visit https://github.com/CATIA-Systems/Modelica-Arduino

39 | ")); 40 | end Arduino; 41 | -------------------------------------------------------------------------------- /Arduino/package.order: -------------------------------------------------------------------------------- 1 | Examples 2 | Components 3 | Firmata 4 | Internal 5 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Simulate Arduino Sketches in Modelica 2 | 3 | With the Arduino Modelica library you can simulate your circuits and [sketches](https://www.arduino.cc/en/Tutorial/Sketch) on a virtual [Arduino Uno](https://www.arduino.cc/en/Main/ArduinoBoardUno) and connect your [Modelica](https://www.modelica.org/) models to real-world circuits using the [Firmata protocol](http://www.firmata.org/). 4 | 5 | ## Prerequisites 6 | 7 | - [Dymola](https://www.3ds.com/products-services/catia/products/dymola) or [OpenModelica](https://www.openmodelica.org/download/download-windows) 8 | - [Visual Studio](https://visualstudio.microsoft.com/downloads/) 2015 or later 9 | - [CMake](https://cmake.org/download/) 10 | 11 | ## Getting Started 12 | 13 | - download the [latest release](https://github.com/CATIA-Systems/Modelica-Arduino/releases/latest) and extract the archive 14 | 15 | ## Simulating a Sketch with Dymola 16 | 17 | - in Dymola select `File > Load...` and open `Arduino/package.mo` from the extracted files 18 | - open `Arduino.Components.ArduinoUno` and set the default values for parameters `cmake` and `generator` to match your installation, e.g. 19 | 20 | ```Modelica 21 | model ArduinoUno "Virtual Arduino Uno" 22 | 23 | // ... 24 | parameter String cmake = "C:\\Program Files\\CMake\\bin\\cmake.exe"; 25 | parameter String generator = "Visual Studio 17 2022"; 26 | // ... 27 | ``` 28 | 29 | - select `File > Save` to save your changes 30 | - open `Arduino.Examples.Blink` 31 | - select the `Simulation` ribbon and click `Simulate` to run the simulation 32 | - after the simulation has finished click the `Play` button in the `Animation Control` section and watch the LED `L` blink 33 | 34 | ![Arduino.Examples.Blink](Arduino/Resources/Images/blink_example.png) 35 | 36 | ## Simulating a Sketch with OpenModelica 37 | 38 | - in OMEdit select `File > Load library` and choose the `Arduino` folder from the extracted files 39 | - open `Arduino.Internal.buildSketchOM` and set the default values for parameters `cmake` and `generator` to match your installation, e.g. 40 | 41 | ```Modelica 42 | encapsulated function buildSketchOM 43 | 44 | // ... 45 | input String cmake = "C:/Program Files/CMake/bin/cmake.exe" "Absolute path to the CMake executable"; 46 | input String generator = "Visual Studio 17 2022" "CMake generator to build the Sketch"; 47 | // ... 48 | ``` 49 | 50 | - select `File > Save` to save your changes 51 | - open `Arduino.Examples.Blink` 52 | - select `Tools > OpenModelica Compiler CLI` and run 53 | 54 | ``` 55 | Arduino.Internal.buildSketchOM("Blink.ino") 56 | ``` 57 | 58 | to build the Arduino binary 59 | 60 | - select `Simulation > Simulate` to run the simulation 61 | - in the `View` toolbar click on the gears icon (Diagram Window) 62 | - in the `Variables Browser` click the play button and watch the LED `L` blink 63 | 64 | ![Arduino.Examples.Blink](Arduino/Resources/Images/blink_example_om.png) 65 | 66 | ## Simulate your own Sketch 67 | 68 | To simulate your sketch `MySketch` you have to perform the following steps. 69 | 70 | - save your sketch as `Arduino/Resources/Sketches/MySketch.ino` 71 | - add the block `Arduino.Components.ArduinoUno` to your model 72 | - double-click the block and set the parameter `sketch` to `"MySketch.ino"` 73 | 74 | In Dymola: 75 | 76 | - click `Translate` to re-translate the model when the sketch has changed 77 | 78 | In OpenModelica: 79 | 80 | - run `Arduino.Internal.buildSketchOM("MySketch.ino")` in the Compiler CLI when the sketch has changed 81 | 82 | ## Limitations 83 | 84 | - currently you can use only one instance of ArduinoUno in your Modelica model 85 | 86 | - the whole [Arduino API](https://www.arduino.cc/en/Reference/HomePage) is supported except for the following functions 87 | 88 | - [analogReadResolution()](https://www.arduino.cc/en/Reference/AnalogReadResolution) 89 | - [analogWriteResolution()](https://www.arduino.cc/en/Reference/AnalogWriteResolution) 90 | - [shiftOut()](https://www.arduino.cc/en/Reference/ShiftOut) 91 | - [shiftIn()](https://www.arduino.cc/en/Reference/ShiftIn) 92 | - [isWhitespace()](https://www.arduino.cc/en/Reference/IsWhitespace) 93 | - [Serial](https://www.arduino.cc/en/Reference/Serial) (`print()`, `println()` and `write()` are supported) 94 | - [Stream](https://www.arduino.cc/en/Reference/Stream) 95 | - [Keyboard](https://www.arduino.cc/en/Reference/MouseKeyboard) 96 | - [Mouse](https://www.arduino.cc/en/Reference/MouseKeyboard) 97 | 98 | ## Arduino Libraries 99 | 100 | The following libraries are included and can be used directly. 101 | 102 | | Name | Description | 103 | |---------------------------------|-------------| 104 | |[Servo.h](Libraries/Servo.h) | [Servo library](https://www.arduino.cc/en/Reference/Servo) to control RC (hobby) servo motors | 105 | |[PID_v1.h](Libraries/PID_v1.h) | PID controller library for Arduino | 106 | 107 | To use an external library in your sketch copy its header files (`*.h`) and source files (`*.cpp`) to the `Libraries` folder. If the library contains `.cpp` files you have to add them to the `Arduino` project by dragging them from the `Libraries` folder in the file browser onto the `Source Files` folder in the solution explorer. 108 | 109 | ## How it works 110 | 111 | The `ArduinoUno` model is driven by an [`ExternalObject`](Arduino/Internal/ExternalArduino.mo) that contains the compiled sketch and an implementation of the [Arduino API](https://www.arduino.cc/reference/en/). The external object is synchronized at every sample step with the Modelica model. 112 | 113 | When a model that contains the `ArduinoUno` block is translated the external object is automatically re-built through the `preInstantiate=Arduino.Internal.buildSketch()` directive in its annotation. 114 | 115 | This function writes a new `Sketch.cpp` to `Arduino/Resources/Source/Arduino` that includes the sketch currently selected in the `ArduinoUno` component, calls CMake to generate the Visual Studio Solution, and builds the shared library `Arduino/Resources/Library/{win32|win64}/ModelicaArduino.dll` that contains the implementation of the external object. 116 | 117 | ## Debugging a Sketch 118 | 119 | - translate and simulate the model you want to debug 120 | - open `Arduino/Resources/Source/Arduino/Win32/Arduino.sln` (or `Arduino/Resources/Source/Arduino/x64/Arduino.sln` if `Advanced.CompileWith64 = 2`) 121 | - select `Debug` as configuration 122 | - open the settings of the `ModelicaArduino` project and under `Debug` set `Command` to `dymosim.exe` and `Working Directory` to your current Dymola working directory 123 | - open the sketch in Visual Studio and set a breakpoint 124 | - start debugging 125 | 126 | ## Copyright and License 127 | 128 | Copyright © 2022 [Dassault Systèmes](https://www.3ds.com/). The code is licensed [GPLv3](https://www.gnu.org/licenses/gpl-3.0.en.html), the documentation [CC BY-SA 4.0](https://creativecommons.org/licenses/by-sa/4.0/). 129 | -------------------------------------------------------------------------------- /dist.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | from subprocess import check_call 3 | 4 | import markdown2 5 | import os 6 | import zipfile 7 | 8 | 9 | archive_name = 'Modelica-Arduino.zip' 10 | 11 | # remove the archive 12 | if os.path.exists(archive_name): 13 | os.remove(archive_name) 14 | 15 | generator = 'Visual Studio 17 2022' 16 | 17 | root = Path(__file__).parent 18 | 19 | # build the ModelicaFirmata library 20 | for platform in ['x64']: 21 | 22 | check_call([ 23 | 'cmake', 24 | '-G', generator, 25 | '-A', platform, 26 | '-S', (root / 'Arduino' / 'Resources' / 'Source' / 'Firmata').as_posix(), 27 | '-B', (root / 'Arduino' / 'Resources' / 'Source' / 'Firmata' / platform).as_posix(), 28 | ]) 29 | 30 | check_call([ 31 | 'cmake', 32 | '--build', (root / 'Arduino' / 'Resources' / 'Source' / 'Firmata' / platform).as_posix(), 33 | '--config', 'Release' 34 | ]) 35 | 36 | dist_files = [] 37 | 38 | input = [('Arduino', ('.mo', '.order', '.png', '.css', '.h', '.c', '.cpp', '.ino', 'ModelicaFirmata.dll', '.wrl', 'CMakeLists.txt'))] 39 | 40 | # collect the distribution files 41 | for folder, suffix in input: 42 | for root, dirs, files in os.walk(folder): 43 | for f in files: 44 | if f.endswith(suffix): 45 | dist_files += [os.path.join(root, f)] 46 | 47 | 48 | dist_files += ['Arduino/Resources/Library/win64', 'LICENSE.txt', 'README.html'] 49 | 50 | 51 | html = """ 52 | 53 | 54 | 55 | 56 | 57 | 58 | """ 59 | 60 | with open('README.md', 'r') as md_file: 61 | html += markdown2.markdown(md_file.read(), extras=['fenced-code-blocks', 'tables']) 62 | 63 | html += """ 64 | 65 | """ 66 | 67 | with open('README.html', 'w') as html_file: 68 | html_file.write(html) 69 | 70 | # create the archive 71 | with zipfile.ZipFile(archive_name, 'w', zipfile.ZIP_DEFLATED) as zipf: 72 | for name in dist_files: 73 | zipf.write(filename=name) 74 | -------------------------------------------------------------------------------- /tests/conftest.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | from os import makedirs 3 | from pathlib import Path 4 | from shutil import rmtree 5 | 6 | 7 | @pytest.fixture 8 | def package(): 9 | yield Path(__file__).parent.parent / 'Arduino' / 'package.mo' 10 | 11 | 12 | @pytest.fixture 13 | def workdir(): 14 | path = Path(__file__).parent / 'work' 15 | rmtree(path, ignore_errors=True) 16 | makedirs(path) 17 | yield path 18 | -------------------------------------------------------------------------------- /tests/results/Arduino.Examples.AnalogReadSerial.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/tests/results/Arduino.Examples.AnalogReadSerial.mat -------------------------------------------------------------------------------- /tests/results/Arduino.Examples.BarGraph.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/tests/results/Arduino.Examples.BarGraph.mat -------------------------------------------------------------------------------- /tests/results/Arduino.Examples.Blink.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/tests/results/Arduino.Examples.Blink.mat -------------------------------------------------------------------------------- /tests/results/Arduino.Examples.BlinkWithoutDelay.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/tests/results/Arduino.Examples.BlinkWithoutDelay.mat -------------------------------------------------------------------------------- /tests/results/Arduino.Examples.Button.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/tests/results/Arduino.Examples.Button.mat -------------------------------------------------------------------------------- /tests/results/Arduino.Examples.ControlledDCMotor.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/tests/results/Arduino.Examples.ControlledDCMotor.mat -------------------------------------------------------------------------------- /tests/results/Arduino.Examples.Fade.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/tests/results/Arduino.Examples.Fade.mat -------------------------------------------------------------------------------- /tests/results/Arduino.Examples.Ping.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/tests/results/Arduino.Examples.Ping.mat -------------------------------------------------------------------------------- /tests/results/Arduino.Examples.RobotArm.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/tests/results/Arduino.Examples.RobotArm.mat -------------------------------------------------------------------------------- /tests/results/Arduino.Examples.ShiftOutHelloWorld.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/tests/results/Arduino.Examples.ShiftOutHelloWorld.mat -------------------------------------------------------------------------------- /tests/results/Arduino.Examples.Sweep.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATIA-Systems/Modelica-Arduino/3e70a5704ae489b7b8912b1f42d6c140ef4eca00/tests/results/Arduino.Examples.Sweep.mat -------------------------------------------------------------------------------- /tests/test_dymola.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | import pytest 3 | import numpy as np 4 | from scipy.io import wavfile 5 | 6 | from tests.utils import validate_result 7 | 8 | 9 | def pymola_available(): 10 | try: 11 | import pymola 12 | return True 13 | except: 14 | return False 15 | 16 | 17 | @pytest.mark.skipif(not pymola_available(), reason="Pymola was not found") 18 | def test_examples(workdir, package): 19 | 20 | from pymola import Dymola 21 | 22 | examples = [ 23 | 'Arduino.Examples.AnalogReadSerial', 24 | 'Arduino.Examples.BarGraph', 25 | 'Arduino.Examples.Blink', 26 | 'Arduino.Examples.BlinkWithoutDelay', 27 | 'Arduino.Examples.Button', 28 | 'Arduino.Examples.ControlledDCMotor', 29 | 'Arduino.Examples.Fade', 30 | 'Arduino.Examples.Ping', 31 | 'Arduino.Examples.RobotArm', 32 | 'Arduino.Examples.ShiftOutHelloWorld', 33 | 'Arduino.Examples.Sweep', 34 | 'Arduino.Examples.ToneMelody', 35 | 'Arduino.Firmata.Examples.Blink', 36 | 'Arduino.Firmata.Examples.Fade', 37 | 'Arduino.Firmata.Examples.AnalogInput', 38 | 'Arduino.Firmata.Examples.Button', 39 | 'Arduino.Firmata.Examples.Sweep', 40 | ] 41 | 42 | with Dymola(showWindow=True) as dymola: 43 | 44 | dymola.openModel(path=package, changeDirectory=False) 45 | 46 | dymola.cd(workdir) 47 | 48 | for model in examples: 49 | 50 | result = dymola.simulate(model) 51 | 52 | assert validate_result( 53 | result=result, 54 | workdir=workdir, 55 | model=model 56 | ) 57 | -------------------------------------------------------------------------------- /tests/test_open_modelica.py: -------------------------------------------------------------------------------- 1 | import os 2 | import pytest 3 | from tests.utils import validate_result 4 | 5 | 6 | def om_available(): 7 | 8 | try: 9 | import OMPython 10 | except: 11 | return False 12 | 13 | return 'OPENMODELICAHOME' in os.environ 14 | 15 | 16 | @pytest.mark.skipif(not om_available(), reason="OpenModelica was not found") 17 | def test_run_examples(workdir, package): 18 | 19 | from OMPython import OMCSessionZMQ 20 | from pymola import Dsres 21 | 22 | omc = OMCSessionZMQ() 23 | 24 | assert omc.sendExpression(f'cd("{workdir.as_posix()}")') == workdir.as_posix() 25 | 26 | assert omc.sendExpression(f'loadFile("{package.as_posix()}")') 27 | 28 | for model, sketch in [ 29 | ('AnalogReadSerial', 'AnalogReadSerial.ino'), 30 | ('BarGraph', 'BarGraph.ino'), 31 | ('Blink', 'Blink.ino'), 32 | ('BlinkWithoutDelay', 'BlinkWithoutDelay.ino'), 33 | ('Button', 'Button.ino'), 34 | ('ControlledDCMotor', 'ControlledDCMotor.ino'), 35 | ('Fade', 'Fade.ino'), 36 | ('Ping', 'Ping.ino'), 37 | #('RobotArm', 'RobotArmGrabSponge.ino'), 38 | ('ShiftOutHelloWorld', 'ShftOut11.ino'), 39 | ('Sweep', 'Sweep.ino'), 40 | ('ToneMelody', 'Melody.ino'), 41 | ]: 42 | assert omc.sendExpression(f'Arduino.Internal.buildSketchOM("{sketch}")') 43 | 44 | info = omc.sendExpression(f'simulate(Arduino.Examples.{model})') 45 | 46 | assert validate_result( 47 | result=Dsres(info['resultFile']), 48 | workdir=workdir, 49 | model=f'Arduino.Examples.{model}' 50 | ) 51 | 52 | for model in ['Blink', 'Fade', 'AnalogInput', 'Button', 'Sweep']: 53 | info = omc.sendExpression(f'simulate(Arduino.Firmata.Examples.{model})') 54 | assert info['resultFile'] 55 | -------------------------------------------------------------------------------- /tests/utils.py: -------------------------------------------------------------------------------- 1 | def validate_signal(t, y, t_ref, y_ref, num=1000, dx=20, dy=0.1): 2 | """ Validate a signal y(t) against a reference signal y_ref(t_ref) by creating a band 3 | around y_ref and finding the values in y outside the band 4 | 5 | Parameters: 6 | 7 | t time of the signal 8 | y values of the signal 9 | t_ref time of the reference signal 10 | y_ref values of the reference signal 11 | num number of samples for the band 12 | dx horizontal width of the band in samples 13 | dy vertical distance of the band to y_ref 14 | 15 | Returns: 16 | 17 | t_band time values of the band 18 | y_min lower limit of the band 19 | y_max upper limit of the band 20 | i_out indices of the values in y outside the band 21 | """ 22 | 23 | import numpy as np 24 | from scipy.ndimage.filters import maximum_filter1d, minimum_filter1d 25 | from scipy.interpolate import interp1d 26 | 27 | # re-sample the reference signal into a uniform grid 28 | t_band = np.linspace(start=t_ref[0], stop=t_ref[-1], num=num) 29 | 30 | # make t_ref strictly monotonic by adding epsilon to duplicate sample times 31 | for i in range(1, len(t_ref)): 32 | while t_ref[i - 1] >= t_ref[i]: 33 | t_ref[i] = t_ref[i] + 1e-13 34 | 35 | interp_method = 'linear' if y.dtype == np.float64 else 'zero' 36 | y_band = interp1d(x=t_ref, y=y_ref, kind=interp_method)(t_band) 37 | 38 | y_band_min = np.min(y_band) 39 | y_band_max = np.max(y_band) 40 | 41 | # calculate the width of the band 42 | if y_band_min == y_band_max: 43 | w = 0.5 if y_band_min == 0 else np.abs(y_band_min) * dy 44 | else: 45 | w = (y_band_max - y_band_min) * dy 46 | 47 | # calculate the lower and upper limits 48 | y_min = minimum_filter1d(input=y_band, size=dx) - w 49 | y_max = maximum_filter1d(input=y_band, size=dx) + w 50 | 51 | # find outliers 52 | y_min_i = np.interp(x=t, xp=t_band, fp=y_min) 53 | y_max_i = np.interp(x=t, xp=t_band, fp=y_max) 54 | i_out = np.logical_or(y < y_min_i, y > y_max_i) 55 | 56 | # do not count outliers outside the t_ref 57 | i_out = np.logical_and(i_out, t > t_band[0]) 58 | i_out = np.logical_and(i_out, t < t_band[-1]) 59 | 60 | return t_band, y_min, y_max, i_out 61 | 62 | 63 | def validate_result(result, workdir, model): 64 | 65 | from pathlib import Path 66 | import numpy as np 67 | import plotly.graph_objects as go 68 | from plotly.subplots import make_subplots 69 | from pymola import Dsres 70 | from scipy.io import wavfile 71 | 72 | if model == 'Arduino.Examples.ToneMelody': 73 | sample_rate, _ = wavfile.read(workdir / 'melody.wav') 74 | assert sample_rate == 44100 75 | 76 | results = Path(__file__).parent / 'results' 77 | 78 | filename = results / f'{model}.mat' 79 | 80 | if not filename.exists(): 81 | return True 82 | 83 | reference = Dsres(filename) 84 | 85 | t = result['Time'] 86 | t_ref = reference['Time'] 87 | 88 | figure = make_subplots(rows=len(reference._names) - 1, cols=1, shared_xaxes=True) 89 | 90 | passed = True 91 | 92 | for i, name in enumerate(reference._names[1:]): 93 | y = result[name] 94 | y_ref = reference[name] 95 | 96 | t_band, y_min, y_max, i_out = validate_signal(t, y, t_ref, y_ref, num=10000, dx=300, dy=0.05) 97 | 98 | passed = passed and not np.any(i_out[10:-10]) 99 | 100 | trace1 = go.Scatter(x=np.concatenate([t_band, t_band[::-1]]), 101 | y=np.concatenate([y_min, y_max[::-1]]), 102 | fill='toself', 103 | line_color='green', 104 | name=f'{name}_ref') 105 | trace2 = go.Scatter(x=t, y=y, line_color='blue', name=name) 106 | 107 | figure.add_trace(trace1, row=i + 1, col=1) 108 | figure.add_trace(trace2, row=i + 1, col=1) 109 | 110 | if np.any(i_out): 111 | trace3 = go.Scatter(x=t[i_out], y=y[i_out], line_color='red', mode='markers', name=f'{name}_out') 112 | figure.add_trace(trace3, row=i + 1, col=1) 113 | 114 | figure.write_html(workdir / f'{model}.html') 115 | 116 | return passed --------------------------------------------------------------------------------