From 0299c012ab849a92b4bbce79305e3548e029418b Mon Sep 17 00:00:00 2001
From: Mael Kerichard <38262536+Pixselve@users.noreply.github.com>
Date: Wed, 3 Jan 2024 19:49:40 +0100
Subject: [PATCH] feat: implement STD

---
 mini-mecha-code/src/compiler/arduino.ts     |  8 +++-
 mini-mecha-code/src/compiler/std-arduino.ts | 53 ++++++++++++++++-----
 2 files changed, 48 insertions(+), 13 deletions(-)

diff --git a/mini-mecha-code/src/compiler/arduino.ts b/mini-mecha-code/src/compiler/arduino.ts
index 3c9d690..5618d6e 100644
--- a/mini-mecha-code/src/compiler/arduino.ts
+++ b/mini-mecha-code/src/compiler/arduino.ts
@@ -59,7 +59,7 @@ export default function compileToArduino(model: Model): string[] {
   }
   result.push("void loop() {");
   // run main function
-  result.push("  main();");
+  result.push("  entry();");
   result.push("}");
   return result;
 }
@@ -76,7 +76,11 @@ function evaluateFunctionDef(functionDef: DefFunction): string[] {
     return [];
 
   const params = functionDef.parameters.map((p) => `float ${p.name}`);
-  result.push(`float ${functionDef.name}(${params.join(", ")}) {`);
+  result.push(
+    `${functionDef.returnType === "void" ? "void" : "float"} ${
+      functionDef.name
+    }(${params.join(", ")}) {`,
+  );
   for (const stmt of functionDef.statements) {
     result.push(...evaluateStatement(stmt, 1));
   }
diff --git a/mini-mecha-code/src/compiler/std-arduino.ts b/mini-mecha-code/src/compiler/std-arduino.ts
index 0dbb851..092c390 100644
--- a/mini-mecha-code/src/compiler/std-arduino.ts
+++ b/mini-mecha-code/src/compiler/std-arduino.ts
@@ -1,10 +1,46 @@
 const stdArduino = `
-void rotate(int angle) {
-  // TODO: Implement
+#include <PinChangeInt.h>
+#include <PinChangeIntConfig.h>
+#include <EEPROM.h>
+#define _NAMIKI_MOTOR\t //for Namiki 22CL-103501PG80:1
+#include <fuzzy_table.h>
+#include <PID_Beta6.h>
+#include <MotorWheel.h>
+#include <Omni4WD.h>
+
+irqISR(irq1, isr1);
+MotorWheel wheel1(3, 2, 4, 5, &irq1);
+
+irqISR(irq2, isr2);
+MotorWheel wheel2(11, 12, 14, 15, &irq2);
+
+irqISR(irq3, isr3);
+MotorWheel wheel3(9, 8, 16, 17, &irq3);
+
+irqISR(irq4, isr4);
+MotorWheel wheel4(10, 7, 18, 19, &irq4);
+
+
+Omni4WD Omni(&wheel1, &wheel2, &wheel3, &wheel4);
+
+
+void setup() {
+  TCCR1B = TCCR1B & 0xf8 | 0x01; // Pin9,Pin10 PWM 31250Hz
+  TCCR2B = TCCR2B & 0xf8 | 0x01; // Pin3,Pin11 PWM 31250Hz
+
+  Omni.PIDEnable(0.31, 0.01, 0, 10);
+}
+
+void rotate(float angle) {
+  Omni.setCarRotate(angle);
 }
 
 void forward(int distance) {
-  // TODO: Implement
+    float speed = Omni.getCarSpeedMMPS();
+    float timeToSleep = distance / speed;
+    Omni.setCarAdvance(speed);
+    Omni.delay(timeToSleep);
+    Omni.setCarStop();
 }
 
 int getDistance() {
@@ -13,16 +49,11 @@ int getDistance() {
 }
 
 int getTimestamp() {
-  // TODO: Implement
-  return 0;
+    return millis();
 }
 
-void setSpeed(int speed) {
-  // TODO: Implement
-}
-
-void setup() {
-  // TODO: Implement
+void setSpeed(float speed) {
+  Omni.setCarSpeedMMPS(speed);
 }
 `;