Diff: main.cpp

Revision:

13:698bd4df9702

Parent:

11:19e8022f60ea

Child:

15:c1a790a1e999

--- a/main.cpp	Thu Nov 24 15:29:50 2016 +0000
+++ b/main.cpp	Fri Feb 08 15:23:40 2019 +0000
@@ -1,154 +1,104 @@
 #include "mbed.h"
 #include "communication_1.h"
 #include "MX106.h"
-#include "MX64.h"
-#include <stdio.h>
-#include <string.h>
+
+// Utility
+InterruptIn button(USER_BUTTON);
+DigitalOut led(LED1);
+
+// Motor Control
+Serial pc(USBTX, USBRX);
+communication_1 wire(PA_9, PA_10, 200000);
+MX106 motor_1(wire, 1, 1);
+MX106 motor_2(wire, 2, 1);
+MX106 motor_3(wire, 3, 1);
 
-#define MAX_JOINT 3 
+void button_int_handler()
+{
+
+}
+
+// CAN
+Thread canrxa;
+
+CAN can1(PA_11, PA_12);     // RX, TX
+
+CANMessage messageIn;
+CANMessage messageOut;
+
+int filter = can1.filter(0x000, 0x400, CANStandard);
 
-/*Connessioni jupers millefori/nucleo: 
-    - rosso 5V
-    - nero GND
-    - marrone D2
-    - verde D8 
-  Il connettore va stacato dal dynamixel e non dalla millefori per non scambiare il verso!*/
-  
-Serial pc(USBTX, USBRX);
-communication_1 wire(PA_9, PA_10, 9600);
-MX106 motor_21(wire, 21, 1);
-MX106 motor_22(wire, 22, 1);
-MX106 motor_14(wire, 14, 1);
+void canrx()
+{
+  while(1)
+  {
+    if(can1.read(messageIn, filter))
+    {
+      printf("CAN: mess %d\n\r", (messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24)));
+      printf("CANaacc: id %x \n\r ",messageIn.id);
+      
+      if((messageIn.id & 0x0FF) == 0x40)
+      {
+          
+      }
+      else if((messageIn.id & 0x0FF) == 0x50)
+      {
+          
+      }
+      else if((messageIn.id & 0x0FF) == 0x60)
+      {
+          
+      }
+      else if((messageIn.id & 0x0FF) == 0x70)
+      {
+          
+      }
+    }
+  }
+}
    
-int main() {  
-   //pc.printf("Start run! \n");
+int main()
+{
    wire.trigger();
    wire.trigger();
    wire.trigger();
    wire.trigger();
    
-   //printf("asking volts\n");
-   //float volts = motor_21.getVolts();
-   //printf("volts: %f\n", volts);
+   // Setup Motor1 MultiTurn
+   motor_1.setMotorEnabled(1);
+   motor_1.setMode(2);
+   motor_1.setSpeed(90);
+   //motor_1.setGoalPosition(0);
+   wait(10);
    
-   motor_21.setMotorEnabled(1);
-
-   //printf("set continuous rotation mode\n");
-   //motor_21.setMode(0);
-   //printf("set speed\n");
-   //motor_21.setSpeed(180);
-   //printf("speed set\n");
-
-
-   //printf("set multiturn mode\n");
-   //motor_21.setMode(2);
-   //printf("asking pos\n");
-   //float pos = motor_21.getPosition();
-   //printf(" pos : %f \n", pos);
-   //motor_21.setSpeed(90);
-   //float wait_time = 5;
-   //motor_21.setGoalPosition(0);
-   //wait(10);
-   //float targets[7] = {0, 90, 180, 270, 180, 360, 720};
-   //for (int i = 0; i < 7; i++) {
-     //motor_21.setGoalPosition(targets[i]);
-     //wait(wait_time);
-   //}
-   //printf("asking pos after move\n");
-   //pos = motor_21.getPosition();
-   //printf(" pos : %f \n", pos);
-
-   Joint* joint_21 = &motor_21;
-
-   float pos;
-   //joint_21->setMaxSpeed(360*3/2);
-   //joint_21->setSpeed(90);
-   //wait(3);
-   //joint_21->setSpeed(0);
-   //wait(1);
-   //pos = motor_21.getPosition();
-   //printf(" pos : %f \n", pos);
+   // Setup Motor2 MultiTurn
+   motor_2.setMotorEnabled(1);
+   motor_2.setMode(2);
+   motor_2.setSpeed(90);
+   //motor_2.setGoalPosition(0);
+   wait(10);
    
-
-   //joint_21->setCWLimit(90);
-   //joint_21->setCCWLimit(-90);
-   //joint_21->setGoalPosition(0); 
-   //wait(5);
-   //pos = motor_21.getPosition();
-   //printf(" pos : %f \n", pos);
-
-   //joint_21->setGoalPosition(80); 
-   //wait(5);
-   //pos = motor_21.getPosition();
-   //printf(" pos : %f \n", pos);
-
-   //joint_21->setGoalPosition(130); 
-   //wait(5);
-   //pos = motor_21.getPosition();
-   //printf(" pos : %f \n", pos);
+   // Setup Motor3 MultiTurn
+   motor_3.setMotorEnabled(1);
+   motor_3.setMode(2);
+   motor_3.setSpeed(90);
+   //motor_3.setGoalPosition(0);
+   wait(10);
 
-   //joint_21->setGoalPosition(-130); 
-   //wait(5);
-   //pos = motor_21.getPosition();
-   //printf(" pos : %f \n", pos);
-   //
-   //
+   printf("DYNAMIXEL: Init DONE\n\r");
    
-   Joint* joints[MAX_JOINT];
-   joints[0] = &motor_21;
-   joints[1] = &motor_22;
-   joints[2] = &motor_14;
-
-   // comandi
-   char buf[200];
-   uint8_t current_motor = 0; 
-
-   while(1) {
-     printf("\n insert command: \n");
-     if(pc.gets(buf, 200)) {
-       char command[3];
-       strncpy(command, buf, 2);
-       command[2]='\0';
+   button.rise(&button_int_handler);
+  
+   // CAN Initialization  
+   canrxa.start(canrx);
 
-       if(strcmp(command, "ID")  == 0) {
-         int id;
-         if(sscanf(buf, "ID %d", &id) != 1) {
-           printf("\n bad ID command \n");
-         };
-         bool found = false;
-         for(int i =0; i< MAX_JOINT && !found; i++) {
-           int nid = joints[i]->getID();
-           printf("found motor with id %d\n", nid);
-           if(id == nid) {
-             printf("\nusing motor %d - ID: %d", i, id);
-             current_motor = i;
-             found = true;
-             break;
-           } 
-         }
-         if(!found) {
-           printf("\nmotor with ID: %d not found", id);
-         }
-       } else if(strcmp(command, "GO") == 0) {
-         float posDegrees;
-         if(sscanf(buf, "GO %f", &posDegrees) != 1) {
-           printf("\n bad GO command \n");
-           break;
-         }
-         joints[current_motor]->setGoalPosition(posDegrees);
-       } else if(strcmp(command, "SP") == 0) {
-         float speed;
-         if(sscanf(buf, "SP %f", &speed) != 1) {
-           printf("\n bad SP command \n");
-           break;
-         }
-         joints[current_motor]->setSpeed(speed);
-       } else {
-         printf("\n unknown command: \"\s\" \n", buf);
-       }
-
-
-     };
+   printf("DONE: CAN Init\n\r");
+  
+  
+   printf("Running!\n\r");
+  
+   while(true)
+   {
+      wait(1000);
    }
-
 }