Motor control for robotic arm
Dependencies: MCP23017 WattBob_TextLCD mbed
Fork of Balanced Arm by
Diff: PC_Comms/PC_Comms.cpp
- Revision:
- 27:d032d8fd4b45
- Parent:
- 26:78282794606d
--- 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); }