Motor control for robotic arm
Dependencies: MCP23017 WattBob_TextLCD mbed
Fork of Balanced Arm by
PC_Comms/PC_Comms.cpp
- Committer:
- wm70
- Date:
- 2016-03-10
- Revision:
- 27:d032d8fd4b45
- Parent:
- 26:78282794606d
File content as of revision 27:d032d8fd4b45:
//********************************************************************* // Author: // File: // Version: // Date: //********************************************************************* #include <string> #include "PC_Comms.h" #include "mbed.h" #include "MCP23017.h" #include "WattBob_TextLCD.h" #include "AX12.h" MCP23017 *par_port; WattBob_TextLCD *lcd; Serial ax12_bus(p28, p27); 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 angle3; char _servo; //********************************************************************* // Object: // Description: //********************************************************************* PC_Comms::PC_Comms() { par_port = new MCP23017(p9, p10, 0x40); par_port->config(0x0F00, 0x0F00, 0x0F00); lcd = new WattBob_TextLCD(par_port); par_port->write_bit(1, BL_BIT); } //********************************************************************* // Method: PC_Connect // Description: This method is used to connect the PC to the mbed. //********************************************************************* void PC_Comms::PC_Connect(void){ my_AX12.SetMode(0); lcd->printf("Wheel Mode"); wait(1); lcd->cls(); lcd->printf("Connecting"); lcd->locate(1,0); lcd->printf("to PC"); while('s' != _PCbus.getc()){ } while('s' != _PCbus.getc()){ } lcd->cls(); lcd->printf("Connected\n"); wait(1); } void PC_Comms::PC_Read(void) { char Command[8]; lcd->cls(); lcd->printf("Reading PC"); if(_PCbus.readable()==1){ Command[0] = _PCbus.getc(); if(Command[0]=='s') { while(_PCbus.readable()==0){} Command[1] = _PCbus.getc(); if(Command[1]=='c') { for(int x=2;x<=7;x++) { while(_PCbus.readable()==0){} Command[x] = _PCbus.getc(); } switch(Command[2]) { case '0': char angle[3]; angle[0] = Command[4]; angle[1] = Command[5]; angle[2] = Command[6]; angle3 = atoi(angle); my_AX12.SetGoal(angle3, '00'); break; case '1': char angle1[3]; angle1[0] = Command[4]; angle1[1] = Command[5]; angle1[2] = Command[6]; 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; } } } } } void PC_Comms::PC_WriteLoad(char _servo){ char _returnToPC[4]; int load_data; load_data = my_AX12.GetLoad(); _returnToPC[0] = 'S'; _returnToPC[1] = 'D'; _returnToPC[2] = '0'; _returnToPC[3] = '1'; if(load_data >= 1000){ _PCbus.printf("%s%d\n",_returnToPC, load_data); }else if(load_data < 1000 && load_data >= 100){ _PCbus.printf("%s0%d\n", _returnToPC, load_data); }else if(load_data < 100 && load_data >= 10){ _PCbus.printf("%s00%d\n", _returnToPC, load_data); }else if(load_data < 10){ _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){ char _returnToPC[4]; int pos_data; pos_data = my_AX12.GetPosition(); _returnToPC[0] = 'S'; _returnToPC[1] = 'D'; _returnToPC[2] = '1'; _returnToPC[3] = _servo; 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); } } 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(){ int load_data; load_data = my_AX12.GetLoad(); if(load_data >= 1070 && 1!= my_AX12.isMoving()){ my_AX12.TorqueEnable(0); } else{ my_AX12.TorqueEnable(1); } }