William McColl
Motor control for robotic arm
Dependencies: MCP23017 WattBob_TextLCD mbed
Fork of
Balanced Arm
by 
Balanced Arm
Revision 27:d032d8fd4b45, committed 2016-03-10
- Comitter:
- wm70
- Date:
- Thu Mar 10 14:39:28 2016 +0000
- Parent:
- 26:78282794606d
- Commit message:
- multiple motors, no feedback;
Changed in this revision
diff -r 78282794606d -r d032d8fd4b45 AX12.cpp
--- a/AX12.cpp Mon Mar 07 18:53:33 2016 +0000
+++ b/AX12.cpp Thu Mar 10 14:39:28 2016 +0000
@@ -36,8 +36,8 @@
SetCCWLimit(0);
SetCRSpeed(0.0);
} else {
- SetCWLimit(70);
- SetCCWLimit(270);
+ SetCWLimit(0);
+ SetCCWLimit(300);
SetCRSpeed(0.0);
}
return(0);
@@ -141,6 +141,16 @@
return (angle);
}
+int AX12::GetVoltage(void){
+
+ char data[2];
+
+ int ErrorCode = read(_ID, AX12_REG_VOLTAGE, 2, data);
+ short volts = data[0] + (data[1] << 8);
+
+ return (volts);
+}
+
int AX12::read(int ID, int start, int bytes, char* data){
char TxBuf[16];
diff -r 78282794606d -r d032d8fd4b45 AX12.h
--- a/AX12.h Mon Mar 07 18:53:33 2016 +0000
+++ b/AX12.h Thu Mar 10 14:39:28 2016 +0000
@@ -13,7 +13,7 @@
#define AX12_REG_GOAL_POSITION 0x1E
#define AX12_REG_MOVING_SPEED 0x20
#define AX12_REG_POSITION 0x24
-#define AX12_REG_VOLTS 0x2A
+#define AX12_REG_VOLTAGE 0x2A
#define AX12_REG_LOAD 0x28
#define AX12_REG_MOVING 0x2E
#define AX12_REG_CW_LIMIT 0x06
@@ -48,6 +48,8 @@
int GetPosition(void);
+ int GetVoltage(void);
+
int isMoving(void);
int SetCWLimit(int degrees);
diff -r 78282794606d -r d032d8fd4b45 PC_Comms/PC_Comms.cpp
--- a/PC_Comms/PC_Comms.cpp Mon Mar 07 18:53:33 2016 +0000
+++ b/PC_Comms/PC_Comms.cpp Thu Mar 10 14:39:28 2016 +0000
@@ -19,8 +19,9 @@
Serial _PCbus(USBTX, USBRX);
AX12 my_AX12(ax12_bus, _PCbus, p29, 1);
AX12 my_AX12_2(ax12_bus, _PCbus, p29, 2);
+ AX12 my_AX12_3(ax12_bus, _PCbus, p29, 3);
- int angle2;
+ int angle3;
char _servo;
@@ -92,9 +93,8 @@
angle[0] = Command[4];
angle[1] = Command[5];
angle[2] = Command[6];
- angle2 = atoi(angle);
- my_AX12.SetGoal(angle2, '00');
- _servo = '1';
+ angle3 = atoi(angle);
+ my_AX12.SetGoal(angle3, '00');
break;
case '1':
@@ -102,9 +102,17 @@
angle1[0] = Command[4];
angle1[1] = Command[5];
angle1[2] = Command[6];
- angle2 = atoi(angle1);
- my_AX12_2.SetGoal(angle2, 'ff');
- _servo = '2';
+ angle3 = atoi(angle1);
+ my_AX12_2.SetGoal(angle3, 'ff');
+ break;
+
+ case '2':
+ char angle2[3];
+ angle2[0] = Command[4];
+ angle2[1] = Command[5];
+ angle2[2] = Command[6];
+ angle3 = atoi(angle2);
+ my_AX12_2.SetGoal(angle3, 'ff');
break;
default:
break;
@@ -123,7 +131,7 @@
_returnToPC[0] = 'S';
_returnToPC[1] = 'D';
_returnToPC[2] = '0';
- _returnToPC[3] = _servo;
+ _returnToPC[3] = '1';
if(load_data >= 1000){
_PCbus.printf("%s%d\n",_returnToPC, load_data);
@@ -135,6 +143,28 @@
_PCbus.printf("%s000%d\n", _returnToPC, load_data);
}
}
+
+ void PC_Comms::PC_WriteLoad_2(char _servo){
+
+ char _returnToPC_1[4];
+ int load_data_1;
+
+ load_data_1 = my_AX12_2.GetLoad();
+ _returnToPC_1[0] = 'S';
+ _returnToPC_1[1] = 'D';
+ _returnToPC_1[2] = '0';
+ _returnToPC_1[3] = '2';
+
+ if(load_data_1 >= 1000){
+ _PCbus.printf("%s%d\n",_returnToPC_1, load_data_1);
+ }else if(load_data_1 < 1000 && load_data_1 >= 100){
+ _PCbus.printf("%s0%d\n", _returnToPC_1, load_data_1);
+ }else if(load_data_1 < 100 && load_data_1 >= 10){
+ _PCbus.printf("%s00%d\n", _returnToPC_1, load_data_1);
+ }else if(load_data_1 < 10){
+ _PCbus.printf("%s000%d\n", _returnToPC_1, load_data_1);
+ }
+}
void PC_Comms::PC_WritePosition(char _servo){
@@ -148,15 +178,38 @@
_returnToPC[3] = _servo;
-// if(pos_data >= 1000){
+ if(pos_data >= 1000){
_PCbus.printf("%s%i\n",_returnToPC,pos_data);
-// }else if(pos_data < 1000 && pos_data >= 100){
-// _PCbus.printf("%s0%i\n", _returnToPC, pos_data);
-// }else if(pos_data < 100 && pos_data >= 10){
-// _PCbus.printf("%s00%i\n", _returnToPC, pos_data);
-// }else{
-// _PCbus.printf("%s000%i\n", _returnToPC, pos_data);
-// }
+ }else if(pos_data < 1000 && pos_data >= 100){
+ _PCbus.printf("%s0%i\n", _returnToPC, pos_data);
+ }else if(pos_data < 100 && pos_data >= 10){
+ _PCbus.printf("%s00%i\n", _returnToPC, pos_data);
+ }else{
+ _PCbus.printf("%s000%i\n", _returnToPC, pos_data);
+ }
+}
+
+void PC_Comms::PC_WriteVoltage(char _servo){
+
+ char _returnToPC[4];
+ int volts_data;
+
+ volts_data = my_AX12.GetVoltage();
+ _returnToPC[0] = 'S';
+ _returnToPC[1] = 'D';
+ _returnToPC[2] = '1';
+ _returnToPC[3] = _servo;
+
+
+ if(volts_data >= 1000){
+ _PCbus.printf("%s%i\n",_returnToPC,volts_data);
+ }else if(volts_data < 1000 && volts_data >= 100){
+ _PCbus.printf("%s0%i\n", _returnToPC, volts_data);
+ }else if(volts_data < 100 && volts_data >= 10){
+ _PCbus.printf("%s00%i\n", _returnToPC, volts_data);
+ }else{
+ _PCbus.printf("%s000%i\n", _returnToPC, volts_data);
+ }
}
void PC_Comms::PC_Human(){
@@ -165,7 +218,7 @@
load_data = my_AX12.GetLoad();
- if(load_data >= 1070&& angle2<=270 && angle2>=70 && 1!= my_AX12.isMoving()){
+ if(load_data >= 1070 && 1!= my_AX12.isMoving()){
my_AX12.TorqueEnable(0);
}
diff -r 78282794606d -r d032d8fd4b45 PC_Comms/PC_Comms.h
--- a/PC_Comms/PC_Comms.h Mon Mar 07 18:53:33 2016 +0000
+++ b/PC_Comms/PC_Comms.h Thu Mar 10 14:39:28 2016 +0000
@@ -15,8 +15,10 @@
void PC_Connect(void);
void PC_Read(void);
- void PC_WriteLoad(char _servo);
+ void PC_WriteLoad(char _servo);
+ void PC_WriteLoad_2(char _servo);
void PC_WritePosition(char _servo);
+ void PC_WriteVoltage(char _servo);
void PC_Human();
private:
diff -r 78282794606d -r d032d8fd4b45 main.cpp
--- a/main.cpp Mon Mar 07 18:53:33 2016 +0000
+++ b/main.cpp Thu Mar 10 14:39:28 2016 +0000
@@ -4,8 +4,6 @@
#include "PC_Comms.h"
PC_Comms HShake;
-
-float position;
int main() {
@@ -17,6 +15,8 @@
HShake.PC_WriteLoad(servo1);
+ HShake.PC_WriteLoad_2(servo1);
+
HShake.PC_WritePosition(servo1);
HShake.PC_Read();