Merge pull request #163 from mcgill-robotics/angular-ui-arm-control-a…

…nd-feedback

Angular UI arm control and feedback

angular_ui_app/src/app/app.component.ts

@@ -7,6 +7,5 @@ import { Component, OnInit } from '@angular/core';
 })
 export class AppComponent implements OnInit {
   ngOnInit(): void {
-
   }
 }

angular_ui_app/src/app/components/arm/arm.component.html

@@ -1,38 +1,76 @@
 <!-- <p>arm-page works! </p> -->
 <!-- <div style="display: flex; flex-wrap: wrap;"> -->
-<app-generic height="235px" width="640px">
-
+<app-generic height="340px" width="500px">
+    <div style="display: flex; justify-content: space-between;">
+        <div>
+            Joystick <button (click)="enableGamepad()">On</button> <button (click)="disableGamepad()">Off</button>
+        </div>
+        <div>Mode: {{last_arm_control_mode}}</div>
+        <div>Sensitivity: {{formatNumber(maxVelPercentage / 0.125)}}</div>
+    </div>
+    <br/>
+    <!-- Delta: 10 -->
     <div class="arm">
         <!-- to solve button problem, chnaged app.comp.ts and app.module.ts -->
-        <div id="armInfo" style="height: 235px; width: 645px;">
-            <h4>Arm Info</h4>
-            <div>
-                <p id="Wrist">Wrist: {{ array[0] }}</p>
-                <div id="buttons">
-                    <button id="wrist1" (click)="increment(0)">+</button>
-                    <button id="wrist2" (click)="decrement(0)">-</button>
-                </div>
-            </div>
-
-            <div>
-                <p id="Elbow">Elbow: {{ array[1] }}</p>
-                <div id="buttons">
-                    <button (click)="increment(1)">+</button>
-                    <button (click)="decrement(1)">-</button>
-                </div>
-            </div>
-
-            <div>
-                <p id="Shoulder">Shoulder: {{ array[2] }}</p>
+        <table>
+            <tr>
+                <td>Elbow</td>
+                <td>{{formatNumber(arm_brushless_FB[0])}}</td>
+                <td style="width: 15%;">
+                    <div id="buttons">
+                        <button id="wrist1" (click)="increment(0)">+</button>
+                        <button id="wrist2" (click)="decrement(0)">-</button>
+                    </div>
+                </td>
+            </tr>
+            <tr>
+                <td>Shoulder</td>
+                <td>{{formatNumber(arm_brushless_FB[1])}}</td>
+                <td style="width: 15%;">
+                    <div id="buttons">
+                        <button (click)="increment(2)">+</button>
+                        <button (click)="decrement(2)">-</button>
+                    </div>
+                </td>
+            </tr>
+            <tr>
+                <td>Tumor</td>
+                <td>{{formatNumber(arm_brushless_FB[2])}}</td>
+                <td style="width: 15%;">
+                    <div id="buttons">
+                        <button (click)="increment(2)">+</button>
+                        <button (click)="decrement(2)">-</button>
+                    </div>
+                </td>
+            </tr>
+            <tr>
+                <td>Wrist Pitch</td>
+                <td>{{formatNumber(arm_brushed_FB[2])}}</td>
+                <td style="width: 15%;">
+                    <div id="buttons">
+                        <button (click)="increment(2)">+</button>
+                        <button (click)="decrement(2)">-</button>
+                    </div>
+                </td>
+            </tr>
+            <tr>
+                <td>Wrist Roll</td>
+                <td>{{formatNumber(arm_brushed_FB[1])}}</td>
+                <td style="width: 15%;">
+                    <div id="buttons">
+                        <button (click)="increment(2)">+</button>
+                        <button (click)="decrement(2)">-</button>
+                    </div>
+                </td>
+            </tr>
+            <td>End Effector</td>
+            <td>{{formatNumber(arm_brushed_FB[0])}}</td>
+            <td style="width: 15%;">
                 <div id="buttons">
                     <button (click)="increment(2)">+</button>
                     <button (click)="decrement(2)">-</button>
                 </div>
-            </div>
-        </div>
+            </td>
+        </table>
     </div>
-
-    <body>
-    </body>
-
 </app-generic>

angular_ui_app/src/app/components/arm/arm.component.scss

@@ -9,9 +9,7 @@ button {
     display: inline-block;
     width:fit-content;
     margin: 0 3px;
-    width: 25px;
-    color-scheme: black;
-    background-color: aqua;
+    background-color: #B6CA7B;
 }
 
 .buttons {
@@ -23,4 +21,17 @@ p {
     margin-top: 20px;
     display: flex;
     height: 10px;
+}
+
+table {
+    width: 96%;
+    margin: 2%;
+    padding: 2px;
+    border: 3px solid gray;
+}
+
+td {
+    padding: 5px;
+    border: 1px solid gray;
+    width: 33.3%;
 }

angular_ui_app/src/app/components/arm/arm.component.ts

@@ -1,4 +1,7 @@
 import { Component, Input, Output, EventEmitter} from '@angular/core';
+import * as ROSLIB from 'roslib';
+import { GamepadService } from 'src/app/gamepad.service';
+import { RosService } from 'src/app/ros.service';
 
 @Component({
   selector: 'app-arm-component',
@@ -10,6 +13,21 @@ export class ArmComponent {
 
   // array contains initial values 
   array  : number[]= [0,0,0];
+  prev_joystick_input_state: { [key: string]: number | boolean } | null = null;
+  last_arm_control_mode: number = 1;
+  maxVelPercentage: number = 0;
+
+  ros: ROSLIB.Ros;
+  gamepad_arm_control_pub: ROSLIB.Topic;
+  arm_brushed_sub: ROSLIB.Topic;
+  arm_brushless_sub: ROSLIB.Topic;
+
+  arm_brushed_FB = [0,0,0];
+  arm_brushless_FB = [0,0,0];
+
+  constructor(private gamepadService: GamepadService, private rosService: RosService) {
+    this.ros = this.rosService.getRos();
+  }
 
   increment(id: number) {
     console.log("id: ", id);
@@ -20,5 +38,97 @@ export class ArmComponent {
   decrement(id: number) {
     this.array[id] -= 0.5;
   }
+
+  ngOnInit() {
+    this.gamepad_arm_control_pub = new ROSLIB.Topic({
+      ros : this.ros,
+      name : '/arm_controller_input',
+      messageType : 'arm_control/ArmControllerInput'
+    });
+
+    this.arm_brushed_sub = new ROSLIB.Topic({
+      ros: this.ros,
+      name: '/armBrushedFB',
+      messageType: 'std_msgs/Float32MultiArray'
+    });
+
+    this.arm_brushless_sub = new ROSLIB.Topic({
+      ros: this.ros,
+      name: '/armBrushlessFB',
+      messageType: 'std_msgs/Float32MultiArray'
+    });
+
+    this.arm_brushed_sub.subscribe((message: any) => {
+      this.arm_brushed_FB = message.data;
+    });
+
+    this.arm_brushless_sub.subscribe((message: any) => {
+      this.arm_brushless_FB = message.data;
+    });
+
+    this.gamepadService.connectJoystickGamepad(
+      (input_dir: { [key: string]: number | boolean }) => {
+        if (this.prev_joystick_input_state == null) {
+          this.prev_joystick_input_state = input_dir;
+        }
+
+        this.maxVelPercentage = (1 - ((input_dir['a4'] as number) + 1) / 2) / 8;
+
+        let msg = {
+          X_dir: (input_dir['a2'] as number) ** 2,
+          Y_dir: (input_dir['a1'] as number) ** 2,
+          Z_dir: (input_dir['a3'] as number) ** 2,
+          MaxVelPercentage: this.maxVelPercentage,
+          Mode: this.last_arm_control_mode,
+        }
+
+        if (input_dir['a2'] as number > 0) {
+          msg.X_dir = -1 * msg.X_dir;
+        }
+
+        if (input_dir['a1'] as number > 0) {
+          msg.Y_dir = -1 * msg.Y_dir;
+        }
+
+        if (input_dir['a3'] as number < 0) {
+          msg.Z_dir = -1 * msg.Z_dir;
+        }
+
+        if (this.risingEdge(this.prev_joystick_input_state['b8'] as boolean, input_dir['b8'] as boolean)) {
+          msg.Mode = 1;
+        } else if (this.risingEdge(this.prev_joystick_input_state['b10'] as boolean, input_dir['b10'] as boolean)) {
+          msg.Mode = 2;
+        } else if (this.risingEdge(this.prev_joystick_input_state['b12'] as boolean, input_dir['b12'] as boolean)) {
+          msg.Mode = 3;
+        } else if (this.risingEdge(this.prev_joystick_input_state['b7'] as boolean, input_dir['b7'] as boolean)) {
+          msg.Mode = 4;
+        } else if (this.risingEdge(this.prev_joystick_input_state['b9'] as boolean, input_dir['b9'] as boolean)) {
+          msg.Mode = 5;
+        } else if (this.risingEdge(this.prev_joystick_input_state['b11'] as boolean, input_dir['b11'] as boolean)) {
+          msg.Mode = 0;
+        }
+
+        this.gamepad_arm_control_pub.publish(msg)
+        this.prev_joystick_input_state = input_dir;
+        this.last_arm_control_mode = msg.Mode;
+      },
+    );
+  }
+
+  private risingEdge(prev: boolean, current: boolean): boolean {
+    return current && !prev;
+  }
+
+  enableGamepad() {
+    this.gamepadService.enableJoystickGamepad();
+  }
+
+  disableGamepad() {
+    this.gamepadService.disableJoystickGamepad();
+  }
+
+  formatNumber(value: number) {
+    return value.toFixed(3);
+  }
 }
 

angular_ui_app/src/app/components/drive/drive.component.html

@@ -1,5 +1,5 @@
 <app-generic height="235px" width="640px">
-    Gamepad <button (click)="enableGamepad()">On</button> <button (click)="disableGamepad()">Off</button>
+    Controller <button (click)="enableGamepad()">On</button> <button (click)="disableGamepad()">Off</button>
     <div class="drive">
         <div id="driveInfo">
             <hr/>

angular_ui_app/src/app/components/drive/drive.component.ts

@@ -11,8 +11,15 @@ import * as ROSLIB from 'roslib';
 export class DriveComponent {
   ros: ROSLIB.Ros;
   gamepad_drive_pub: ROSLIB.Topic;
+  pan_tilt_pub: ROSLIB.Topic;
+
   gagamepadService: GamepadService;
 
+  pantilt_yaw: number = 0;
+  pantilt_pitch: number = 90;
+
+  angle_delta: number = 0.5;
+
   constructor(private gamepadService: GamepadService, private rosService: RosService) {
     this.ros = this.rosService.getRos();
   }
@@ -23,18 +30,44 @@ export class DriveComponent {
       name : '/angular_ui_app/drive',
       messageType : 'std_msgs/Float32MultiArray'
     });
-  }
 
-  enableGamepad() {
-    this.gamepadService.enableGamepad(
-      (axis_v) => {
-        this.gamepad_drive_pub.publish({data: [-axis_v[1], axis_v[2]]});
+    this.pan_tilt_pub = new ROSLIB.Topic({
+      ros: this.ros,
+      name: '/pantiltCmd',
+      messageType: 'std_msgs/Float32MultiArray'
+    })
+
+    this.gamepadService.connectControllerGamepad(
+      (input_dir: { [key: string]: number | boolean }) => {
+        this.gamepad_drive_pub.publish({data: [input_dir['a2'], input_dir['a4']]});
+
+        if (input_dir['up']) {
+          this.pantilt_pitch = this.clamp(this.pantilt_pitch + this.angle_delta);
+        }
+        if (input_dir['down']) {
+          this.pantilt_pitch = this.clamp(this.pantilt_pitch - this.angle_delta);
+        }
+        if (input_dir['left']) {
+          this.pantilt_yaw = this.clamp(this.pantilt_yaw + this.angle_delta);
+        }
+        if (input_dir['right']) {
+          this.pantilt_yaw = this.clamp(this.pantilt_yaw - this.angle_delta);;
+        }
+
+        this.pan_tilt_pub.publish({data: [this.pantilt_pitch, this.pantilt_yaw]})
       },
-      (button_v, i) => {}
     );
   }
 
+  enableGamepad() {
+    this.gamepadService.enableControllerGamepad();
+  }
+
   disableGamepad() {
-    this.gamepadService.disableGamepad();
+    this.gamepadService.disableControllerGamepad();
+  }
+
+  private clamp(new_angle: number) {
+    return Math.max(0, Math.min(new_angle, 180))
   }
 }

angular_ui_app/src/app/components/generic/generic.component.html

@@ -1,3 +1,3 @@
-<div style="border: 1px solid black; background-color: #2A3F47;" [ngStyle]="{ height: height, width: width}">
+<div style="border: 1px solid black; background-color: #2A3F47; margin-bottom: 5px;" [ngStyle]="{ height: height, width: width}">
     <ng-content></ng-content>
 </div>