Motor control for robotic arm
Dependencies: MCP23017 WattBob_TextLCD mbed
Fork of Balanced Arm by
Diff: HandShake/HandShake.cpp
- Revision:
- 11:7f74fb17c328
- Parent:
- 10:d7cce35b7bfd
--- a/HandShake/HandShake.cpp Fri Jan 29 17:38:18 2016 +0000 +++ b/HandShake/HandShake.cpp Mon Feb 01 15:48:29 2016 +0000 @@ -10,30 +10,25 @@ #include "mbed.h" #include "MCP23017.h" #include "WattBob_TextLCD.h" - #include "AX12.h" - - - - MCP23017 *par_port1; - WattBob_TextLCD *lcd1; + + MCP23017 *par_port; + WattBob_TextLCD *lcd; + Serial ax12_bus(p28, p27); + AX12 my_AX12(ax12_bus, p29, 1); //********************************************************************* // Object: PC_HandShake // Description: To create a HandShake object. //********************************************************************* -PC_HandShake::PC_HandShake(Serial& PCbus, AX12& ax12) - : _PCbus(PCbus), _ax12 (ax12){ +PC_HandShake::PC_HandShake(Serial& PCbus) + : _PCbus(PCbus){ -//********************************************************************* -// Description:Setting up the LCD display and turning on LCD display -// Pins: p8, p10 -//********************************************************************* - par_port1 = new MCP23017(p9, p10, 0x40); - par_port1->config(0x0F00, 0x0F00, 0x0F00); - lcd1 = new WattBob_TextLCD(par_port1); - par_port1->write_bit(1,BL_BIT); + 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); } //********************************************************************* @@ -42,77 +37,84 @@ //********************************************************************* void PC_HandShake::PC_Connect(void){ - lcd1->cls(); - lcd1->printf("Connecting"); - lcd1->locate(1,0); - lcd1->printf("to PC"); - wait(2); - lcd1->cls(); - lcd1->printf("Waiting\n"); + lcd->cls(); + lcd->printf("Connecting"); + lcd->locate(1,0); + lcd->printf("to PC"); while('s' != _PCbus.getc()){ } while('s' != _PCbus.getc()){ } - lcd1->cls(); - lcd1->printf("Conneced\n"); + lcd->cls(); + lcd->printf("Conneced\n"); wait(2); } void PC_HandShake::PC_Read(void){ - lcd1->cls(); - lcd1->printf("Reading"); - lcd1->locate(1,0); - lcd1->printf("to PC"); - wait(2); - lcd1->cls(); - lcd1->printf("Waiting\n"); + char Command[8]; - int x; + + lcd->cls(); + lcd->printf("Reading PC"); while(_PCbus.readable()==0){} - for(x=0;x<=8;x++) + 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(); } - if(Command[0]=='s') - { - if (Command[1]=='c') - if(Command[2]=='1') - _ax12.SetGoal(100); - - else if (Command[2]=='0') - _ax12.SetGoal(0); - - else return; + lcd->cls(); + lcd->printf("%s", Command); + wait(1); + switch(Command[2]){ + case '0': + char angle[3]; + angle[0] = Command[4]; + angle[1] = Command[5]; + angle[2] = Command[6]; + int angle2 = atoi(angle); + lcd->cls(); + lcd->printf("%i", angle2); + my_AX12.SetGoal(angle2); + break; + default: + break; + } + } + wait(1); + } +} - } +void PC_HandShake::PC_WriteLoad(char _servo){ - //while('s' != _PCbus.getc()){ -// } -// while(_PCbus.readable()==0){} -// char _byteOne=_PCbus.getc(); -// -// while(_PCbus.readable()==0){} -// char _byteTwo=_PCbus.getc(); -// -// if (_byteOne=='s'){ -// } -// else if (_byteOne=='c') -// { -// switch(_byteTwo) -// { -// case '0': -// _ax12.SetGoal(100); -// break; -// case '1': -// _ax12.SetGoal(0); -// break; -// } -// } - - - lcd1->cls(); - lcd1->printf("Reading\n"); - wait(2); + char _data[3]; + char _returnToPC[8]; + + lcd->cls(); + lcd->printf("Writing to PC"); + + my_AX12.GetLoad(_data); + + //lcd->cls(); +// lcd->printf("%s", _data); + + _returnToPC[0] = 'S'; + _returnToPC[1] = 'D'; + _returnToPC[2] = '0'; + _returnToPC[3] = _servo; + _returnToPC[4] = _data[0]; + _returnToPC[5] = _data[1]; + _returnToPC[6] = _data[2]; + _returnToPC[7] = '0'; + + _PCbus.printf(_returnToPC); + wait(1); } \ No newline at end of file