Motor control for robotic arm
Dependencies: MCP23017 WattBob_TextLCD mbed
Fork of Balanced Arm by
HandShake/HandShake.cpp@11:7f74fb17c328, 2016-02-01 (annotated)
- Committer:
- ADAMSTRUTT
- Date:
- Mon Feb 01 15:48:29 2016 +0000
- Revision:
- 11:7f74fb17c328
- Parent:
- 10:d7cce35b7bfd
Torque almost working
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ADAMSTRUTT | 7:40d7d88deba3 | 1 | //********************************************************************* |
ADAMSTRUTT | 7:40d7d88deba3 | 2 | // Author: Adam Strutt |
ADAMSTRUTT | 7:40d7d88deba3 | 3 | // File: (PS2C.cpp) PlayStation 2 controller |
ADAMSTRUTT | 7:40d7d88deba3 | 4 | // Version: 1.0v |
ADAMSTRUTT | 7:40d7d88deba3 | 5 | // Date: 11/12/2015 |
ADAMSTRUTT | 7:40d7d88deba3 | 6 | //********************************************************************* |
ADAMSTRUTT | 7:40d7d88deba3 | 7 | |
ADAMSTRUTT | 7:40d7d88deba3 | 8 | #include <string> |
ADAMSTRUTT | 7:40d7d88deba3 | 9 | #include "HandShake.h" |
ADAMSTRUTT | 7:40d7d88deba3 | 10 | #include "mbed.h" |
ADAMSTRUTT | 7:40d7d88deba3 | 11 | #include "MCP23017.h" |
ADAMSTRUTT | 7:40d7d88deba3 | 12 | #include "WattBob_TextLCD.h" |
wm70 | 9:87558e7cfecb | 13 | #include "AX12.h" |
ADAMSTRUTT | 11:7f74fb17c328 | 14 | |
ADAMSTRUTT | 11:7f74fb17c328 | 15 | MCP23017 *par_port; |
ADAMSTRUTT | 11:7f74fb17c328 | 16 | WattBob_TextLCD *lcd; |
ADAMSTRUTT | 11:7f74fb17c328 | 17 | Serial ax12_bus(p28, p27); |
ADAMSTRUTT | 11:7f74fb17c328 | 18 | AX12 my_AX12(ax12_bus, p29, 1); |
ADAMSTRUTT | 7:40d7d88deba3 | 19 | |
ADAMSTRUTT | 7:40d7d88deba3 | 20 | //********************************************************************* |
ADAMSTRUTT | 7:40d7d88deba3 | 21 | // Object: PC_HandShake |
ADAMSTRUTT | 7:40d7d88deba3 | 22 | // Description: To create a HandShake object. |
ADAMSTRUTT | 7:40d7d88deba3 | 23 | //********************************************************************* |
ADAMSTRUTT | 7:40d7d88deba3 | 24 | |
ADAMSTRUTT | 11:7f74fb17c328 | 25 | PC_HandShake::PC_HandShake(Serial& PCbus) |
ADAMSTRUTT | 11:7f74fb17c328 | 26 | : _PCbus(PCbus){ |
ADAMSTRUTT | 7:40d7d88deba3 | 27 | |
ADAMSTRUTT | 11:7f74fb17c328 | 28 | par_port = new MCP23017(p9, p10, 0x40); |
ADAMSTRUTT | 11:7f74fb17c328 | 29 | par_port->config(0x0F00, 0x0F00, 0x0F00); |
ADAMSTRUTT | 11:7f74fb17c328 | 30 | lcd = new WattBob_TextLCD(par_port); |
ADAMSTRUTT | 11:7f74fb17c328 | 31 | par_port->write_bit(1, BL_BIT); |
ADAMSTRUTT | 7:40d7d88deba3 | 32 | } |
ADAMSTRUTT | 7:40d7d88deba3 | 33 | |
ADAMSTRUTT | 7:40d7d88deba3 | 34 | //********************************************************************* |
ADAMSTRUTT | 7:40d7d88deba3 | 35 | // Method: PC_Connect |
ADAMSTRUTT | 7:40d7d88deba3 | 36 | // Description: This method is used to connect the PC to the mbed. |
ADAMSTRUTT | 7:40d7d88deba3 | 37 | //********************************************************************* |
ADAMSTRUTT | 7:40d7d88deba3 | 38 | |
ADAMSTRUTT | 7:40d7d88deba3 | 39 | void PC_HandShake::PC_Connect(void){ |
ADAMSTRUTT | 11:7f74fb17c328 | 40 | lcd->cls(); |
ADAMSTRUTT | 11:7f74fb17c328 | 41 | lcd->printf("Connecting"); |
ADAMSTRUTT | 11:7f74fb17c328 | 42 | lcd->locate(1,0); |
ADAMSTRUTT | 11:7f74fb17c328 | 43 | lcd->printf("to PC"); |
Hamilton252 | 10:d7cce35b7bfd | 44 | while('s' != _PCbus.getc()){ |
ADAMSTRUTT | 7:40d7d88deba3 | 45 | } |
Hamilton252 | 10:d7cce35b7bfd | 46 | while('s' != _PCbus.getc()){ |
ADAMSTRUTT | 7:40d7d88deba3 | 47 | } |
ADAMSTRUTT | 11:7f74fb17c328 | 48 | lcd->cls(); |
ADAMSTRUTT | 11:7f74fb17c328 | 49 | lcd->printf("Conneced\n"); |
ADAMSTRUTT | 7:40d7d88deba3 | 50 | wait(2); |
wm70 | 9:87558e7cfecb | 51 | } |
wm70 | 9:87558e7cfecb | 52 | |
wm70 | 9:87558e7cfecb | 53 | void PC_HandShake::PC_Read(void){ |
ADAMSTRUTT | 11:7f74fb17c328 | 54 | |
Hamilton252 | 10:d7cce35b7bfd | 55 | char Command[8]; |
ADAMSTRUTT | 11:7f74fb17c328 | 56 | |
ADAMSTRUTT | 11:7f74fb17c328 | 57 | lcd->cls(); |
ADAMSTRUTT | 11:7f74fb17c328 | 58 | lcd->printf("Reading PC"); |
wm70 | 9:87558e7cfecb | 59 | |
wm70 | 9:87558e7cfecb | 60 | while(_PCbus.readable()==0){} |
ADAMSTRUTT | 11:7f74fb17c328 | 61 | Command[0] = _PCbus.getc(); |
ADAMSTRUTT | 11:7f74fb17c328 | 62 | if(Command[0]=='s') |
Hamilton252 | 10:d7cce35b7bfd | 63 | { |
ADAMSTRUTT | 11:7f74fb17c328 | 64 | while(_PCbus.readable()==0){} |
ADAMSTRUTT | 11:7f74fb17c328 | 65 | Command[1] = _PCbus.getc(); |
ADAMSTRUTT | 11:7f74fb17c328 | 66 | if(Command[1]=='c') |
ADAMSTRUTT | 11:7f74fb17c328 | 67 | { |
ADAMSTRUTT | 11:7f74fb17c328 | 68 | for(int x=2;x<=7;x++) |
ADAMSTRUTT | 11:7f74fb17c328 | 69 | { |
ADAMSTRUTT | 11:7f74fb17c328 | 70 | while(_PCbus.readable()==0){} |
Hamilton252 | 10:d7cce35b7bfd | 71 | Command[x] = _PCbus.getc(); |
Hamilton252 | 10:d7cce35b7bfd | 72 | } |
Hamilton252 | 10:d7cce35b7bfd | 73 | |
ADAMSTRUTT | 11:7f74fb17c328 | 74 | lcd->cls(); |
ADAMSTRUTT | 11:7f74fb17c328 | 75 | lcd->printf("%s", Command); |
ADAMSTRUTT | 11:7f74fb17c328 | 76 | wait(1); |
ADAMSTRUTT | 11:7f74fb17c328 | 77 | switch(Command[2]){ |
ADAMSTRUTT | 11:7f74fb17c328 | 78 | case '0': |
ADAMSTRUTT | 11:7f74fb17c328 | 79 | char angle[3]; |
ADAMSTRUTT | 11:7f74fb17c328 | 80 | angle[0] = Command[4]; |
ADAMSTRUTT | 11:7f74fb17c328 | 81 | angle[1] = Command[5]; |
ADAMSTRUTT | 11:7f74fb17c328 | 82 | angle[2] = Command[6]; |
ADAMSTRUTT | 11:7f74fb17c328 | 83 | int angle2 = atoi(angle); |
ADAMSTRUTT | 11:7f74fb17c328 | 84 | lcd->cls(); |
ADAMSTRUTT | 11:7f74fb17c328 | 85 | lcd->printf("%i", angle2); |
ADAMSTRUTT | 11:7f74fb17c328 | 86 | my_AX12.SetGoal(angle2); |
ADAMSTRUTT | 11:7f74fb17c328 | 87 | break; |
ADAMSTRUTT | 11:7f74fb17c328 | 88 | default: |
ADAMSTRUTT | 11:7f74fb17c328 | 89 | break; |
ADAMSTRUTT | 11:7f74fb17c328 | 90 | } |
ADAMSTRUTT | 11:7f74fb17c328 | 91 | } |
ADAMSTRUTT | 11:7f74fb17c328 | 92 | wait(1); |
ADAMSTRUTT | 11:7f74fb17c328 | 93 | } |
ADAMSTRUTT | 11:7f74fb17c328 | 94 | } |
Hamilton252 | 10:d7cce35b7bfd | 95 | |
ADAMSTRUTT | 11:7f74fb17c328 | 96 | void PC_HandShake::PC_WriteLoad(char _servo){ |
wm70 | 9:87558e7cfecb | 97 | |
ADAMSTRUTT | 11:7f74fb17c328 | 98 | char _data[3]; |
ADAMSTRUTT | 11:7f74fb17c328 | 99 | char _returnToPC[8]; |
ADAMSTRUTT | 11:7f74fb17c328 | 100 | |
ADAMSTRUTT | 11:7f74fb17c328 | 101 | lcd->cls(); |
ADAMSTRUTT | 11:7f74fb17c328 | 102 | lcd->printf("Writing to PC"); |
ADAMSTRUTT | 11:7f74fb17c328 | 103 | |
ADAMSTRUTT | 11:7f74fb17c328 | 104 | my_AX12.GetLoad(_data); |
ADAMSTRUTT | 11:7f74fb17c328 | 105 | |
ADAMSTRUTT | 11:7f74fb17c328 | 106 | //lcd->cls(); |
ADAMSTRUTT | 11:7f74fb17c328 | 107 | // lcd->printf("%s", _data); |
ADAMSTRUTT | 11:7f74fb17c328 | 108 | |
ADAMSTRUTT | 11:7f74fb17c328 | 109 | _returnToPC[0] = 'S'; |
ADAMSTRUTT | 11:7f74fb17c328 | 110 | _returnToPC[1] = 'D'; |
ADAMSTRUTT | 11:7f74fb17c328 | 111 | _returnToPC[2] = '0'; |
ADAMSTRUTT | 11:7f74fb17c328 | 112 | _returnToPC[3] = _servo; |
ADAMSTRUTT | 11:7f74fb17c328 | 113 | _returnToPC[4] = _data[0]; |
ADAMSTRUTT | 11:7f74fb17c328 | 114 | _returnToPC[5] = _data[1]; |
ADAMSTRUTT | 11:7f74fb17c328 | 115 | _returnToPC[6] = _data[2]; |
ADAMSTRUTT | 11:7f74fb17c328 | 116 | _returnToPC[7] = '0'; |
ADAMSTRUTT | 11:7f74fb17c328 | 117 | |
ADAMSTRUTT | 11:7f74fb17c328 | 118 | _PCbus.printf(_returnToPC); |
ADAMSTRUTT | 11:7f74fb17c328 | 119 | wait(1); |
ADAMSTRUTT | 7:40d7d88deba3 | 120 | } |