Debugged library and example main
Dependencies: MX106_not_working mbed-src AX12_final comunication_1
Fork of MX106-custom by
main.cpp@11:19e8022f60ea, 2016-07-13 (annotated)
- Committer:
- clynamen
- Date:
- Wed Jul 13 15:53:34 2016 +0000
- Revision:
- 11:19e8022f60ea
- Parent:
- 10:2acfa1a84c96
do fixes;
Who changed what in which revision?
User | Revision | Line number | New 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 | } |