Team DIANA / dynamixel

d

Dependencies:   AX12_final MX106_not_working comunication_1 mbed-dev

Fork of MX106-finaltest by Team DIANA

main.cpp@12:6ac3cac5677c, 2016-11-24 (annotated)

Committer:
clynamen
Date:
Thu Nov 24 15:29:50 2016 +0000
Revision:
12:6ac3cac5677c
Parent:
11:19e8022f60ea 
dynamixel

Who changed what in which revision?

UserRevisionLine numberNew contents of line
clynamen 0:23acabab7c11 1 #include "mbed.h"
mattiasub 6:941fdda9d110 2 #include "communication_1.h"
dconsoli 4:6450eb95727d 3 #include "MX106.h"
clynamen 10:2acfa1a84c96 4 #include "MX64.h"
clynamen 11:19e8022f60ea 5 #include <stdio.h>
clynamen 11:19e8022f60ea 6 #include <string.h>
clynamen 11:19e8022f60ea 7
clynamen 11:19e8022f60ea 8 #define MAX_JOINT 3
dconsoli 4:6450eb95727d 9
dconsoli 3:d2c3ac534549 10 /*Connessioni jupers millefori/nucleo:
dconsoli 3:d2c3ac534549 11 - rosso 5V
dconsoli 3:d2c3ac534549 12 - nero GND
dconsoli 3:d2c3ac534549 13 - marrone D2
dconsoli 3:d2c3ac534549 14 - verde D8
dconsoli 3:d2c3ac534549 15 Il connettore va stacato dal dynamixel e non dalla millefori per non scambiare il verso!*/
dconsoli 3:d2c3ac534549 16
dconsoli 3:d2c3ac534549 17 Serial pc(USBTX, USBRX);
mattiasub 6:941fdda9d110 18 communication_1 wire(PA_9, PA_10, 9600);
clynamen 10:2acfa1a84c96 19 MX106 motor_21(wire, 21, 1);
clynamen 11:19e8022f60ea 20 MX106 motor_22(wire, 22, 1);
clynamen 11:19e8022f60ea 21 MX106 motor_14(wire, 14, 1);
ilaria 1:946c65496383 22
dconsoli 3:d2c3ac534549 23 int main() {
clynamen 11:19e8022f60ea 24 //pc.printf("Start run! \n");
clynamen 11:19e8022f60ea 25 wire.trigger();
clynamen 11:19e8022f60ea 26 wire.trigger();
clynamen 11:19e8022f60ea 27 wire.trigger();
clynamen 11:19e8022f60ea 28 wire.trigger();
clynamen 11:19e8022f60ea 29
clynamen 11:19e8022f60ea 30 //printf("asking volts\n");
clynamen 11:19e8022f60ea 31 //float volts = motor_21.getVolts();
clynamen 11:19e8022f60ea 32 //printf("volts: %f\n", volts);
clynamen 11:19e8022f60ea 33
clynamen 11:19e8022f60ea 34 motor_21.setMotorEnabled(1);
clynamen 11:19e8022f60ea 35
clynamen 11:19e8022f60ea 36 //printf("set continuous rotation mode\n");
clynamen 11:19e8022f60ea 37 //motor_21.setMode(0);
clynamen 11:19e8022f60ea 38 //printf("set speed\n");
clynamen 11:19e8022f60ea 39 //motor_21.setSpeed(180);
clynamen 11:19e8022f60ea 40 //printf("speed set\n");
clynamen 11:19e8022f60ea 41
clynamen 11:19e8022f60ea 42
clynamen 11:19e8022f60ea 43 //printf("set multiturn mode\n");
clynamen 11:19e8022f60ea 44 //motor_21.setMode(2);
clynamen 11:19e8022f60ea 45 //printf("asking pos\n");
clynamen 11:19e8022f60ea 46 //float pos = motor_21.getPosition();
clynamen 11:19e8022f60ea 47 //printf(" pos : %f \n", pos);
clynamen 11:19e8022f60ea 48 //motor_21.setSpeed(90);
clynamen 11:19e8022f60ea 49 //float wait_time = 5;
clynamen 11:19e8022f60ea 50 //motor_21.setGoalPosition(0);
clynamen 11:19e8022f60ea 51 //wait(10);
clynamen 11:19e8022f60ea 52 //float targets[7] = {0, 90, 180, 270, 180, 360, 720};
clynamen 11:19e8022f60ea 53 //for (int i = 0; i < 7; i++) {
clynamen 11:19e8022f60ea 54 //motor_21.setGoalPosition(targets[i]);
clynamen 11:19e8022f60ea 55 //wait(wait_time);
clynamen 11:19e8022f60ea 56 //}
clynamen 11:19e8022f60ea 57 //printf("asking pos after move\n");
clynamen 11:19e8022f60ea 58 //pos = motor_21.getPosition();
clynamen 11:19e8022f60ea 59 //printf(" pos : %f \n", pos);
clynamen 11:19e8022f60ea 60
clynamen 11:19e8022f60ea 61 Joint* joint_21 = &motor_21;
clynamen 11:19e8022f60ea 62
clynamen 11:19e8022f60ea 63 float pos;
clynamen 11:19e8022f60ea 64 //joint_21->setMaxSpeed(360*3/2);
clynamen 11:19e8022f60ea 65 //joint_21->setSpeed(90);
clynamen 11:19e8022f60ea 66 //wait(3);
clynamen 11:19e8022f60ea 67 //joint_21->setSpeed(0);
clynamen 11:19e8022f60ea 68 //wait(1);
clynamen 11:19e8022f60ea 69 //pos = motor_21.getPosition();
clynamen 11:19e8022f60ea 70 //printf(" pos : %f \n", pos);
clynamen 11:19e8022f60ea 71
clynamen 11:19e8022f60ea 72
clynamen 11:19e8022f60ea 73 //joint_21->setCWLimit(90);
clynamen 11:19e8022f60ea 74 //joint_21->setCCWLimit(-90);
clynamen 11:19e8022f60ea 75 //joint_21->setGoalPosition(0);
clynamen 11:19e8022f60ea 76 //wait(5);
clynamen 11:19e8022f60ea 77 //pos = motor_21.getPosition();
clynamen 11:19e8022f60ea 78 //printf(" pos : %f \n", pos);
clynamen 11:19e8022f60ea 79
clynamen 11:19e8022f60ea 80 //joint_21->setGoalPosition(80);
clynamen 11:19e8022f60ea 81 //wait(5);
clynamen 11:19e8022f60ea 82 //pos = motor_21.getPosition();
clynamen 11:19e8022f60ea 83 //printf(" pos : %f \n", pos);
clynamen 11:19e8022f60ea 84
clynamen 11:19e8022f60ea 85 //joint_21->setGoalPosition(130);
clynamen 11:19e8022f60ea 86 //wait(5);
clynamen 11:19e8022f60ea 87 //pos = motor_21.getPosition();
clynamen 11:19e8022f60ea 88 //printf(" pos : %f \n", pos);
clynamen 11:19e8022f60ea 89
clynamen 11:19e8022f60ea 90 //joint_21->setGoalPosition(-130);
clynamen 11:19e8022f60ea 91 //wait(5);
clynamen 11:19e8022f60ea 92 //pos = motor_21.getPosition();
clynamen 11:19e8022f60ea 93 //printf(" pos : %f \n", pos);
clynamen 11:19e8022f60ea 94 //
clynamen 11:19e8022f60ea 95 //
clynamen 11:19e8022f60ea 96
clynamen 11:19e8022f60ea 97 Joint* joints[MAX_JOINT];
clynamen 11:19e8022f60ea 98 joints[0] = &motor_21;
clynamen 11:19e8022f60ea 99 joints[1] = &motor_22;
clynamen 11:19e8022f60ea 100 joints[2] = &motor_14;
clynamen 11:19e8022f60ea 101
clynamen 11:19e8022f60ea 102 // comandi
clynamen 11:19e8022f60ea 103 char buf[200];
clynamen 11:19e8022f60ea 104 uint8_t current_motor = 0;
clynamen 11:19e8022f60ea 105
clynamen 11:19e8022f60ea 106 while(1) {
clynamen 11:19e8022f60ea 107 printf("\n insert command: \n");
clynamen 11:19e8022f60ea 108 if(pc.gets(buf, 200)) {
clynamen 11:19e8022f60ea 109 char command[3];
clynamen 11:19e8022f60ea 110 strncpy(command, buf, 2);
clynamen 11:19e8022f60ea 111 command[2]='\0';
clynamen 11:19e8022f60ea 112
clynamen 11:19e8022f60ea 113 if(strcmp(command, "ID") == 0) {
clynamen 11:19e8022f60ea 114 int id;
clynamen 11:19e8022f60ea 115 if(sscanf(buf, "ID %d", &id) != 1) {
clynamen 11:19e8022f60ea 116 printf("\n bad ID command \n");
clynamen 11:19e8022f60ea 117 };
clynamen 11:19e8022f60ea 118 bool found = false;
clynamen 11:19e8022f60ea 119 for(int i =0; i< MAX_JOINT && !found; i++) {
clynamen 11:19e8022f60ea 120 int nid = joints[i]->getID();
clynamen 11:19e8022f60ea 121 printf("found motor with id %d\n", nid);
clynamen 11:19e8022f60ea 122 if(id == nid) {
clynamen 11:19e8022f60ea 123 printf("\nusing motor %d - ID: %d", i, id);
clynamen 11:19e8022f60ea 124 current_motor = i;
clynamen 11:19e8022f60ea 125 found = true;
clynamen 11:19e8022f60ea 126 break;
clynamen 11:19e8022f60ea 127 }
clynamen 11:19e8022f60ea 128 }
clynamen 11:19e8022f60ea 129 if(!found) {
clynamen 11:19e8022f60ea 130 printf("\nmotor with ID: %d not found", id);
clynamen 11:19e8022f60ea 131 }
clynamen 11:19e8022f60ea 132 } else if(strcmp(command, "GO") == 0) {
clynamen 11:19e8022f60ea 133 float posDegrees;
clynamen 11:19e8022f60ea 134 if(sscanf(buf, "GO %f", &posDegrees) != 1) {
clynamen 11:19e8022f60ea 135 printf("\n bad GO command \n");
clynamen 11:19e8022f60ea 136 break;
clynamen 11:19e8022f60ea 137 }
clynamen 11:19e8022f60ea 138 joints[current_motor]->setGoalPosition(posDegrees);
clynamen 11:19e8022f60ea 139 } else if(strcmp(command, "SP") == 0) {
clynamen 11:19e8022f60ea 140 float speed;
clynamen 11:19e8022f60ea 141 if(sscanf(buf, "SP %f", &speed) != 1) {
clynamen 11:19e8022f60ea 142 printf("\n bad SP command \n");
clynamen 11:19e8022f60ea 143 break;
clynamen 11:19e8022f60ea 144 }
clynamen 11:19e8022f60ea 145 joints[current_motor]->setSpeed(speed);
clynamen 11:19e8022f60ea 146 } else {
clynamen 11:19e8022f60ea 147 printf("\n unknown command: \"\s\" \n", buf);
clynamen 11:19e8022f60ea 148 }
clynamen 11:19e8022f60ea 149
clynamen 11:19e8022f60ea 150
clynamen 11:19e8022f60ea 151 };
clynamen 11:19e8022f60ea 152 }
clynamen 11:19e8022f60ea 153
clynamen 10:2acfa1a84c96 154 }