main.cpp

00001 #include "mbed.h"
00002 #include "communication_1.h"
00003 #include "MX106.h"
00004 #include "MX64.h"
00005 #include <stdio.h>
00006 #include <string.h>
00007 
00008 #define MAX_JOINT 3 
00009 
00010 /*Connessioni jupers millefori/nucleo: 
00011     - rosso 5V
00012     - nero GND
00013     - marrone D2
00014     - verde D8 
00015   Il connettore va stacato dal dynamixel e non dalla millefori per non scambiare il verso!*/
00016   
00017 Serial pc(USBTX, USBRX);
00018 communication_1 wire(PA_9, PA_10, 9600);
00019 MX106 motor_21(wire, 21, 1);
00020 MX106 motor_22(wire, 22, 1);
00021 MX106 motor_14(wire, 14, 1);
00022    
00023 int main() {  
00024    //pc.printf("Start run! \n");
00025    wire.trigger();
00026    wire.trigger();
00027    wire.trigger();
00028    wire.trigger();
00029    
00030    //printf("asking volts\n");
00031    //float volts = motor_21.getVolts();
00032    //printf("volts: %f\n", volts);
00033    
00034    motor_21.setMotorEnabled(1);
00035 
00036    //printf("set continuous rotation mode\n");
00037    //motor_21.setMode(0);
00038    //printf("set speed\n");
00039    //motor_21.setSpeed(180);
00040    //printf("speed set\n");
00041 
00042 
00043    //printf("set multiturn mode\n");
00044    //motor_21.setMode(2);
00045    //printf("asking pos\n");
00046    //float pos = motor_21.getPosition();
00047    //printf(" pos : %f \n", pos);
00048    //motor_21.setSpeed(90);
00049    //float wait_time = 5;
00050    //motor_21.setGoalPosition(0);
00051    //wait(10);
00052    //float targets[7] = {0, 90, 180, 270, 180, 360, 720};
00053    //for (int i = 0; i < 7; i++) {
00054      //motor_21.setGoalPosition(targets[i]);
00055      //wait(wait_time);
00056    //}
00057    //printf("asking pos after move\n");
00058    //pos = motor_21.getPosition();
00059    //printf(" pos : %f \n", pos);
00060 
00061    Joint* joint_21 = &motor_21;
00062 
00063    float pos;
00064    //joint_21->setMaxSpeed(360*3/2);
00065    //joint_21->setSpeed(90);
00066    //wait(3);
00067    //joint_21->setSpeed(0);
00068    //wait(1);
00069    //pos = motor_21.getPosition();
00070    //printf(" pos : %f \n", pos);
00071    
00072 
00073    //joint_21->setCWLimit(90);
00074    //joint_21->setCCWLimit(-90);
00075    //joint_21->setGoalPosition(0); 
00076    //wait(5);
00077    //pos = motor_21.getPosition();
00078    //printf(" pos : %f \n", pos);
00079 
00080    //joint_21->setGoalPosition(80); 
00081    //wait(5);
00082    //pos = motor_21.getPosition();
00083    //printf(" pos : %f \n", pos);
00084 
00085    //joint_21->setGoalPosition(130); 
00086    //wait(5);
00087    //pos = motor_21.getPosition();
00088    //printf(" pos : %f \n", pos);
00089 
00090    //joint_21->setGoalPosition(-130); 
00091    //wait(5);
00092    //pos = motor_21.getPosition();
00093    //printf(" pos : %f \n", pos);
00094    //
00095    //
00096    
00097    Joint* joints[MAX_JOINT];
00098    joints[0] = &motor_21;
00099    joints[1] = &motor_22;
00100    joints[2] = &motor_14;
00101 
00102    // comandi
00103    char buf[200];
00104    uint8_t current_motor = 0; 
00105 
00106    while(1) {
00107      printf("\n insert command: \n");
00108      if(pc.gets(buf, 200)) {
00109        char command[3];
00110        strncpy(command, buf, 2);
00111        command[2]='\0';
00112 
00113        if(strcmp(command, "ID")  == 0) {
00114          int id;
00115          if(sscanf(buf, "ID %d", &id) != 1) {
00116            printf("\n bad ID command \n");
00117          };
00118          bool found = false;
00119          for(int i =0; i< MAX_JOINT && !found; i++) {
00120            int nid = joints[i]->getID();
00121            printf("found motor with id %d\n", nid);
00122            if(id == nid) {
00123              printf("\nusing motor %d - ID: %d", i, id);
00124              current_motor = i;
00125              found = true;
00126              break;
00127            } 
00128          }
00129          if(!found) {
00130            printf("\nmotor with ID: %d not found", id);
00131          }
00132        } else if(strcmp(command, "GO") == 0) {
00133          float posDegrees;
00134          if(sscanf(buf, "GO %f", &posDegrees) != 1) {
00135            printf("\n bad GO command \n");
00136            break;
00137          }
00138          joints[current_motor]->setGoalPosition(posDegrees);
00139        } else if(strcmp(command, "SP") == 0) {
00140          float speed;
00141          if(sscanf(buf, "SP %f", &speed) != 1) {
00142            printf("\n bad SP command \n");
00143            break;
00144          }
00145          joints[current_motor]->setSpeed(speed);
00146        } else {
00147          printf("\n unknown command: \"\s\" \n", buf);
00148        }
00149 
00150 
00151      };
00152    }
00153 
00154 }