added corner ultrasonic sensor positions + adjusted speed of robot

Author: tawjaw

demo/src/index.css

@@ -67,14 +67,16 @@ body {
 
 
 
-canvas { 
+canvas {
   height : auto;
-  width: 100%; 
+  width: 100%;
   max-width: 900px;
+
 }
 
 
 .robot-environment{
   margin: 20px;
+
 }
 

demo/src/index.ts

@@ -136,8 +136,11 @@ function initiateRobot(){
     const canvas = document.getElementById('world');
 
     const robot = new Robots.Arduino.TwoServoRobot(canvas)
-    robot.addUltrasonicSensor('L2', 2,3);
+    robot.addUltrasonicSensor('CFL', 2,3);
     robot.addUltrasonicSensor('F2', 4,5);
+    robot.addUltrasonicSensor('CFR', 2,3);
+    robot.addUltrasonicSensor('CBR', 4,5);robot.addUltrasonicSensor('CFL', 2,3);
+    robot.addUltrasonicSensor('CBL', 4,5);robot.addUltrasonicSensor('CFL', 2,3);
     robot.environment.setRobotInitialPosition({x:50, y: 100});
     robot.environment?.addObstacleRectangle(400, 50, 800, 20, "grey");
     robot.environment?.addObstacleRectangle(700, 200, 20, 800, "grey");

demo/tsconfig.json

@@ -4,7 +4,7 @@
       "target": "es2018",
       "baseUrl": "..",
       "rootDir": "..",
-      "lib": ["dom"],
+      "lib": ["dom", "ES2015"],
       "paths": {
         "p4robots": ["src"]
       }

src/Arduino/Uno/execute.ts

@@ -4,6 +4,7 @@ import {
   CPU,
   timer0Config,
   timer1Config,
+  timer2Config,
   AVRIOPort,
   AVRUSART,
   portBConfig,
@@ -12,7 +13,7 @@ import {
   usart0Config
 } from 'avr8js';
 import { loadHex } from './intelhex';
-import { MicroTaskScheduler } from './task-scheduler';
+
 type Event = {
   period : number,  //milliseconds
   eventCall : any
@@ -26,13 +27,13 @@ export class AVRRunner {
   readonly cpu: CPU;
   readonly timer0: AVRTimer;
   readonly timer1: AVRTimer;
+  readonly timer2: AVRTimer;
   readonly portB: AVRIOPort;
   readonly portC: AVRIOPort;
   readonly portD: AVRIOPort;
   readonly usart: AVRUSART;
   readonly speed = 16e6; // 16 MHZ
   readonly workUnitCycles = 500000;
-  readonly taskScheduler = new MicroTaskScheduler();
 
 
   //events
@@ -44,11 +45,11 @@ export class AVRRunner {
     this.cpu = new CPU(this.program);
     this.timer0 = new AVRTimer(this.cpu, timer0Config);
     this.timer1 = new AVRTimer(this.cpu, timer1Config);
+    this.timer2 = new AVRTimer(this.cpu, timer2Config);
     this.portB = new AVRIOPort(this.cpu, portBConfig);
     this.portC = new AVRIOPort(this.cpu, portCConfig);
     this.portD = new AVRIOPort(this.cpu, portDConfig);
     this.usart = new AVRUSART(this.cpu, usart0Config, this.speed);
-    this.taskScheduler.start();
   }
 
   private cpuTimeMS = 0;

src/Arduino/Uno/task-scheduler.spec.ts

@@ -28,7 +28,7 @@ export class TwoServoRobot {
 
         //add arduino events
         //update the wheel speeds from servo components
-        this.arduino.addCPUEvent(5, () => {
+        this.arduino.addCPUEvent(100, () => {
             const leftServoSpeed = (this.servoLeft.getWidthOfLastPulse() - 1.4);
             const rightServoSpeed = -1*(this.servoRight.getWidthOfLastPulse() - 1.4);
             this.environment?.setSpeeds(leftServoSpeed, rightServoSpeed);
@@ -43,12 +43,11 @@ export class TwoServoRobot {
                 this.ultrasonicSensors[key].sensor.setDistanceOfObstacle(this.environment.ultrasonicSensorDistances[key]);
             }
 
+            this.environment?.tick(50);
 
-        })
-        this.arduino.addCPUEvent(50, () => {
-             this.environment?.tick(1000);
 
         })
+
         this.arduino.addCPUEventMicrosecond(5, (cpuCycles : number) => {
             if(this.environment)
             {
@@ -77,7 +76,6 @@ export class TwoServoRobot {
     run(hex: string)
     {
         this.environment?.reset();
-        this.environment?.tick(100);
         for(const key in this.ultrasonicSensors){
             this.ultrasonicSensors[key].sensor.setDistanceOfObstacle(this.environment.ultrasonicSensorDistances[key]);
         }

src/Arduino/Uno/task-scheduler.ts

@@ -1,9 +1,21 @@
-import {Body, Engine, Render, Bodies, World, Vector, Runner, Events, Mouse, MouseConstraint} from "matter-js";
+import {
+    Body,
+    Engine,
+    Render,
+    Bodies,
+    World,
+    Vector,
+    Runner,
+    Events,
+    Mouse,
+    MouseConstraint,
+    Composite
+} from "matter-js";
 import {createPartCircle} from "./utils/CustomBodies"
 import { getTranformedPoint, findMinimumDistanceToObstacle } from "./utils/utils";
 
 type event = (robot : TwoWheelRobot) => any;
-export type sensorPosition = 'L1'| 'L2' | 'L3' | 'F1' | 'F2' | 'F3' | 'R1' | 'R2' | 'R3' | 'B1' | 'B2'| 'B3';
+export type sensorPosition = 'L1'| 'L2' | 'L3' | 'F1' | 'F2' | 'F3' | 'R1' | 'R2' | 'R3' | 'B1' | 'B2'| 'B3' | 'CFL' | 'CFR' | 'CBL' | 'CBR';
 export class TwoWheelRobot {
     private _canvas : any;
     private _engine : Engine;
@@ -17,8 +29,8 @@ export class TwoWheelRobot {
     robotInitialAngle : number;
 
     robotMass : number = 1000;
-    robotFrictionAir:number = 0.5;
-    static readonly forceMultiplier = 0.0015;
+    robotFrictionAir:number = 1;
+    static readonly forceMultiplier = 0.5;
 
     ultrasonicSensor : Body;
     static readonly maxUltrasonicDistance = 400;
@@ -34,7 +46,12 @@ export class TwoWheelRobot {
         'F3': {x: 14, y: 9, angle: 0},
         'B1': {x: -14, y:-9, angle: -Math.PI},
         'B2': {x: -14, y: 0, angle: -Math.PI},
-        'B3': {x: -14, y: 9, angle: -Math.PI}
+        'B3': {x: -14, y: 9, angle: -Math.PI},
+        'CFL': {x: 14, y:-9, angle: Math.PI/4},
+        'CFR': {x: 14, y: 9, angle: -Math.PI/4},
+        'CBL': {x: -14, y:-9, angle: -5*Math.PI/4},
+        'CBR': {x: -14, y: 9, angle: 5*Math.PI/4},
+
     }
     ultrasonicSensorDistances : { [position: string]: number; } = {};
 
@@ -99,6 +116,19 @@ export class TwoWheelRobot {
 
         World.add(this._engine.world, [this.robot, ]);//obstacle]);
 
+        /*
+        // add mouse control
+        const mouse = Mouse.create(this._canvas);
+
+         const mouseConstraint = MouseConstraint.create(this._engine, {
+                mouse: mouse
+            });
+
+        World.add(this._engine.world, mouseConstraint);
+        // keep the mouse in sync with rendering
+        this._render.mouse = mouse;
+        */
+
         Render.run(this._render);
         this.reset();
 
@@ -130,8 +160,17 @@ export class TwoWheelRobot {
 
                 }
             }
+            /*
+            //Add event with 'mousemove'
+            Events.on(mouseConstraint, 'mousemove', function (event) {
+                //For Matter.Query.point pass "array of bodies" and "mouse position"
+
+                //Your custom code here
+                console.log(event); //returns a shape corrisponding to the mouse position
 
+            });
 
+             */
             self.updateUltrasonicSensor();
 
 
@@ -208,24 +247,30 @@ export class TwoWheelRobot {
         this.leftWheelSpeed = left;
         this.rightWheelSpeed = right;
     }
-    private once = 0;
+    //TODO review if this is actually solving a bug or not
+    private starting = true;
     applyForces() : void {
-
+    if(!this.starting) {
         const leftForcePosition = getTranformedPoint(this.robotBody.position, this.robot.angle, 0, -10);
         const rightForcePosition = getTranformedPoint(this.robotBody.position, this.robot.angle, 0, 10);
-        let leftWheelForce = Vector.create(TwoWheelRobot.forceMultiplier*Math.abs(this.leftWheelSpeed), 0);
+        let leftWheelForce = Vector.create(TwoWheelRobot.forceMultiplier * Math.abs(this.leftWheelSpeed), 0);
         leftWheelForce = Vector.rotate(leftWheelForce, this.robot.angle);
-        if(this.leftWheelSpeed < 0)
+        if (this.leftWheelSpeed < 0)
             leftWheelForce = Vector.neg(leftWheelForce);
 
-        let rightWheelForce = Vector.create(TwoWheelRobot.forceMultiplier*Math.abs(this.rightWheelSpeed), 0);
+        let rightWheelForce = Vector.create(TwoWheelRobot.forceMultiplier * Math.abs(this.rightWheelSpeed), 0);
         rightWheelForce = Vector.rotate(rightWheelForce, this.robot.angle);
-        if(this.rightWheelSpeed < 0)
+        if (this.rightWheelSpeed < 0)
             rightWheelForce = Vector.neg(rightWheelForce);
 
-        Body.applyForce(this.robot, rightForcePosition, rightWheelForce);
         Body.applyForce(this.robot, leftForcePosition, leftWheelForce);
+        Body.applyForce(this.robot, rightForcePosition, rightWheelForce);
+    }
 
+    else
+    {
+        this.starting = false;
+    }
 
     }
 
@@ -253,6 +298,7 @@ export class TwoWheelRobot {
         Body.setAngle(this.robot, this.robotInitialAngle);
         Body.setVelocity(this.robot, {x:0,y:0});
         Body.setAngularVelocity(this.robot,0);
+        Body.update(this.robot, 10, 1,1);
         this.leftWheelSpeed = 0;
         this.rightWheelSpeed = 0;
         for(const coin of this.removedCoins)
@@ -261,7 +307,7 @@ export class TwoWheelRobot {
         }
         this.removedCoins = [];
 
-        this.tick(10);
+        //this.tick(10);
         this.updateUltrasonicSensor();
 
     }