Motor control for robotic arm
Dependencies: MCP23017 WattBob_TextLCD mbed
Fork of Balanced Arm by
PC_Comms/PC_Comms.cpp@26:78282794606d, 2016-03-07 (annotated)
- Committer:
- wm70
- Date:
- Mon Mar 07 18:53:33 2016 +0000
- Revision:
- 26:78282794606d
- Parent:
- 25:d345ef4cb2b5
- Child:
- 27:d032d8fd4b45
Feedback - no; multiple motors;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ADAMSTRUTT | 7:40d7d88deba3 | 1 | //********************************************************************* |
wm70 | 12:68f82c3e27e9 | 2 | // Author: |
wm70 | 12:68f82c3e27e9 | 3 | // File: |
wm70 | 12:68f82c3e27e9 | 4 | // Version: |
wm70 | 12:68f82c3e27e9 | 5 | // Date: |
ADAMSTRUTT | 7:40d7d88deba3 | 6 | //********************************************************************* |
ADAMSTRUTT | 7:40d7d88deba3 | 7 | |
ADAMSTRUTT | 7:40d7d88deba3 | 8 | #include <string> |
wm70 | 12:68f82c3e27e9 | 9 | #include "PC_Comms.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 | |
wm70 | 25:d345ef4cb2b5 | 15 | |
ADAMSTRUTT | 11:7f74fb17c328 | 16 | MCP23017 *par_port; |
ADAMSTRUTT | 11:7f74fb17c328 | 17 | WattBob_TextLCD *lcd; |
ADAMSTRUTT | 11:7f74fb17c328 | 18 | Serial ax12_bus(p28, p27); |
wm70 | 14:3be8ca0c066d | 19 | Serial _PCbus(USBTX, USBRX); |
wm70 | 14:3be8ca0c066d | 20 | AX12 my_AX12(ax12_bus, _PCbus, p29, 1); |
wm70 | 25:d345ef4cb2b5 | 21 | AX12 my_AX12_2(ax12_bus, _PCbus, p29, 2); |
wm70 | 26:78282794606d | 22 | |
wm70 | 25:d345ef4cb2b5 | 23 | int angle2; |
wm70 | 26:78282794606d | 24 | char _servo; |
ADAMSTRUTT | 23:c1c20aee64a0 | 25 | |
ADAMSTRUTT | 7:40d7d88deba3 | 26 | |
ADAMSTRUTT | 7:40d7d88deba3 | 27 | //********************************************************************* |
wm70 | 12:68f82c3e27e9 | 28 | // Object: |
wm70 | 12:68f82c3e27e9 | 29 | // Description: |
ADAMSTRUTT | 7:40d7d88deba3 | 30 | //********************************************************************* |
ADAMSTRUTT | 7:40d7d88deba3 | 31 | |
wm70 | 14:3be8ca0c066d | 32 | PC_Comms::PC_Comms() |
wm70 | 14:3be8ca0c066d | 33 | { |
ADAMSTRUTT | 7:40d7d88deba3 | 34 | |
ADAMSTRUTT | 11:7f74fb17c328 | 35 | par_port = new MCP23017(p9, p10, 0x40); |
ADAMSTRUTT | 11:7f74fb17c328 | 36 | par_port->config(0x0F00, 0x0F00, 0x0F00); |
ADAMSTRUTT | 11:7f74fb17c328 | 37 | lcd = new WattBob_TextLCD(par_port); |
ADAMSTRUTT | 11:7f74fb17c328 | 38 | par_port->write_bit(1, BL_BIT); |
ADAMSTRUTT | 7:40d7d88deba3 | 39 | } |
ADAMSTRUTT | 7:40d7d88deba3 | 40 | |
ADAMSTRUTT | 7:40d7d88deba3 | 41 | //********************************************************************* |
ADAMSTRUTT | 7:40d7d88deba3 | 42 | // Method: PC_Connect |
ADAMSTRUTT | 7:40d7d88deba3 | 43 | // Description: This method is used to connect the PC to the mbed. |
ADAMSTRUTT | 7:40d7d88deba3 | 44 | //********************************************************************* |
ADAMSTRUTT | 7:40d7d88deba3 | 45 | |
wm70 | 12:68f82c3e27e9 | 46 | void PC_Comms::PC_Connect(void){ |
wm70 | 18:b79a9a7cf5fb | 47 | |
ADAMSTRUTT | 23:c1c20aee64a0 | 48 | my_AX12.SetMode(0); |
wm70 | 24:462afd19d12e | 49 | lcd->printf("Wheel Mode"); |
ADAMSTRUTT | 23:c1c20aee64a0 | 50 | |
wm70 | 18:b79a9a7cf5fb | 51 | wait(1); |
wm70 | 18:b79a9a7cf5fb | 52 | |
ADAMSTRUTT | 11:7f74fb17c328 | 53 | lcd->cls(); |
ADAMSTRUTT | 11:7f74fb17c328 | 54 | lcd->printf("Connecting"); |
ADAMSTRUTT | 11:7f74fb17c328 | 55 | lcd->locate(1,0); |
ADAMSTRUTT | 11:7f74fb17c328 | 56 | lcd->printf("to PC"); |
Hamilton252 | 10:d7cce35b7bfd | 57 | while('s' != _PCbus.getc()){ |
ADAMSTRUTT | 7:40d7d88deba3 | 58 | } |
Hamilton252 | 10:d7cce35b7bfd | 59 | while('s' != _PCbus.getc()){ |
ADAMSTRUTT | 7:40d7d88deba3 | 60 | } |
ADAMSTRUTT | 11:7f74fb17c328 | 61 | lcd->cls(); |
wm70 | 18:b79a9a7cf5fb | 62 | lcd->printf("Connected\n"); |
wm70 | 24:462afd19d12e | 63 | wait(1); |
wm70 | 9:87558e7cfecb | 64 | } |
wm70 | 9:87558e7cfecb | 65 | |
wm70 | 26:78282794606d | 66 | void PC_Comms::PC_Read(void) |
wm70 | 26:78282794606d | 67 | { |
ADAMSTRUTT | 11:7f74fb17c328 | 68 | |
Hamilton252 | 10:d7cce35b7bfd | 69 | char Command[8]; |
ADAMSTRUTT | 11:7f74fb17c328 | 70 | |
ADAMSTRUTT | 11:7f74fb17c328 | 71 | lcd->cls(); |
ADAMSTRUTT | 11:7f74fb17c328 | 72 | lcd->printf("Reading PC"); |
wm70 | 9:87558e7cfecb | 73 | |
wm70 | 18:b79a9a7cf5fb | 74 | if(_PCbus.readable()==1){ |
ADAMSTRUTT | 11:7f74fb17c328 | 75 | Command[0] = _PCbus.getc(); |
ADAMSTRUTT | 11:7f74fb17c328 | 76 | if(Command[0]=='s') |
Hamilton252 | 10:d7cce35b7bfd | 77 | { |
ADAMSTRUTT | 11:7f74fb17c328 | 78 | while(_PCbus.readable()==0){} |
ADAMSTRUTT | 11:7f74fb17c328 | 79 | Command[1] = _PCbus.getc(); |
ADAMSTRUTT | 11:7f74fb17c328 | 80 | if(Command[1]=='c') |
ADAMSTRUTT | 11:7f74fb17c328 | 81 | { |
wm70 | 12:68f82c3e27e9 | 82 | for(int x=2;x<=7;x++) |
wm70 | 12:68f82c3e27e9 | 83 | { |
wm70 | 12:68f82c3e27e9 | 84 | while(_PCbus.readable()==0){} |
wm70 | 12:68f82c3e27e9 | 85 | Command[x] = _PCbus.getc(); |
wm70 | 12:68f82c3e27e9 | 86 | } |
wm70 | 24:462afd19d12e | 87 | |
wm70 | 12:68f82c3e27e9 | 88 | switch(Command[2]) |
wm70 | 12:68f82c3e27e9 | 89 | { |
ADAMSTRUTT | 11:7f74fb17c328 | 90 | case '0': |
ADAMSTRUTT | 16:585f8b5abc2c | 91 | char angle[3]; |
ADAMSTRUTT | 16:585f8b5abc2c | 92 | angle[0] = Command[4]; |
ADAMSTRUTT | 16:585f8b5abc2c | 93 | angle[1] = Command[5]; |
ADAMSTRUTT | 16:585f8b5abc2c | 94 | angle[2] = Command[6]; |
wm70 | 25:d345ef4cb2b5 | 95 | angle2 = atoi(angle); |
wm70 | 26:78282794606d | 96 | my_AX12.SetGoal(angle2, '00'); |
wm70 | 26:78282794606d | 97 | _servo = '1'; |
ADAMSTRUTT | 16:585f8b5abc2c | 98 | break; |
wm70 | 25:d345ef4cb2b5 | 99 | |
wm70 | 25:d345ef4cb2b5 | 100 | case '1': |
wm70 | 25:d345ef4cb2b5 | 101 | char angle1[3]; |
wm70 | 25:d345ef4cb2b5 | 102 | angle1[0] = Command[4]; |
wm70 | 25:d345ef4cb2b5 | 103 | angle1[1] = Command[5]; |
wm70 | 25:d345ef4cb2b5 | 104 | angle1[2] = Command[6]; |
wm70 | 25:d345ef4cb2b5 | 105 | angle2 = atoi(angle1); |
wm70 | 26:78282794606d | 106 | my_AX12_2.SetGoal(angle2, 'ff'); |
wm70 | 26:78282794606d | 107 | _servo = '2'; |
wm70 | 25:d345ef4cb2b5 | 108 | break; |
ADAMSTRUTT | 11:7f74fb17c328 | 109 | default: |
ADAMSTRUTT | 11:7f74fb17c328 | 110 | break; |
wm70 | 12:68f82c3e27e9 | 111 | } |
ADAMSTRUTT | 11:7f74fb17c328 | 112 | } |
ADAMSTRUTT | 11:7f74fb17c328 | 113 | } |
ADAMSTRUTT | 11:7f74fb17c328 | 114 | } |
wm70 | 18:b79a9a7cf5fb | 115 | } |
Hamilton252 | 10:d7cce35b7bfd | 116 | |
wm70 | 12:68f82c3e27e9 | 117 | void PC_Comms::PC_WriteLoad(char _servo){ |
ADAMSTRUTT | 16:585f8b5abc2c | 118 | |
wm70 | 14:3be8ca0c066d | 119 | char _returnToPC[4]; |
wm70 | 17:258067827672 | 120 | int load_data; |
ADAMSTRUTT | 11:7f74fb17c328 | 121 | |
wm70 | 17:258067827672 | 122 | load_data = my_AX12.GetLoad(); |
ADAMSTRUTT | 16:585f8b5abc2c | 123 | _returnToPC[0] = 'S'; |
ADAMSTRUTT | 16:585f8b5abc2c | 124 | _returnToPC[1] = 'D'; |
ADAMSTRUTT | 16:585f8b5abc2c | 125 | _returnToPC[2] = '0'; |
ADAMSTRUTT | 16:585f8b5abc2c | 126 | _returnToPC[3] = _servo; |
ADAMSTRUTT | 11:7f74fb17c328 | 127 | |
wm70 | 17:258067827672 | 128 | if(load_data >= 1000){ |
wm70 | 19:bf64727d5b0e | 129 | _PCbus.printf("%s%d\n",_returnToPC, load_data); |
wm70 | 19:bf64727d5b0e | 130 | }else if(load_data < 1000 && load_data >= 100){ |
wm70 | 19:bf64727d5b0e | 131 | _PCbus.printf("%s0%d\n", _returnToPC, load_data); |
wm70 | 19:bf64727d5b0e | 132 | }else if(load_data < 100 && load_data >= 10){ |
wm70 | 19:bf64727d5b0e | 133 | _PCbus.printf("%s00%d\n", _returnToPC, load_data); |
wm70 | 19:bf64727d5b0e | 134 | }else if(load_data < 10){ |
wm70 | 19:bf64727d5b0e | 135 | _PCbus.printf("%s000%d\n", _returnToPC, load_data); |
ADAMSTRUTT | 16:585f8b5abc2c | 136 | } |
wm70 | 18:b79a9a7cf5fb | 137 | } |
wm70 | 18:b79a9a7cf5fb | 138 | |
wm70 | 18:b79a9a7cf5fb | 139 | void PC_Comms::PC_WritePosition(char _servo){ |
wm70 | 18:b79a9a7cf5fb | 140 | |
wm70 | 20:d93f0af76c7a | 141 | char _returnToPC[4]; |
wm70 | 20:d93f0af76c7a | 142 | int pos_data; |
wm70 | 20:d93f0af76c7a | 143 | |
wm70 | 20:d93f0af76c7a | 144 | pos_data = my_AX12.GetPosition(); |
wm70 | 20:d93f0af76c7a | 145 | _returnToPC[0] = 'S'; |
wm70 | 20:d93f0af76c7a | 146 | _returnToPC[1] = 'D'; |
wm70 | 20:d93f0af76c7a | 147 | _returnToPC[2] = '1'; |
wm70 | 20:d93f0af76c7a | 148 | _returnToPC[3] = _servo; |
wm70 | 20:d93f0af76c7a | 149 | |
wm70 | 24:462afd19d12e | 150 | |
wm70 | 24:462afd19d12e | 151 | // if(pos_data >= 1000){ |
wm70 | 24:462afd19d12e | 152 | _PCbus.printf("%s%i\n",_returnToPC,pos_data); |
wm70 | 24:462afd19d12e | 153 | // }else if(pos_data < 1000 && pos_data >= 100){ |
wm70 | 24:462afd19d12e | 154 | // _PCbus.printf("%s0%i\n", _returnToPC, pos_data); |
wm70 | 24:462afd19d12e | 155 | // }else if(pos_data < 100 && pos_data >= 10){ |
wm70 | 24:462afd19d12e | 156 | // _PCbus.printf("%s00%i\n", _returnToPC, pos_data); |
wm70 | 24:462afd19d12e | 157 | // }else{ |
wm70 | 24:462afd19d12e | 158 | // _PCbus.printf("%s000%i\n", _returnToPC, pos_data); |
wm70 | 24:462afd19d12e | 159 | // } |
wm70 | 20:d93f0af76c7a | 160 | } |
wm70 | 24:462afd19d12e | 161 | |
ADAMSTRUTT | 23:c1c20aee64a0 | 162 | void PC_Comms::PC_Human(){ |
ADAMSTRUTT | 23:c1c20aee64a0 | 163 | |
ADAMSTRUTT | 23:c1c20aee64a0 | 164 | int load_data; |
ADAMSTRUTT | 23:c1c20aee64a0 | 165 | |
ADAMSTRUTT | 23:c1c20aee64a0 | 166 | load_data = my_AX12.GetLoad(); |
ADAMSTRUTT | 23:c1c20aee64a0 | 167 | |
wm70 | 24:462afd19d12e | 168 | if(load_data >= 1070&& angle2<=270 && angle2>=70 && 1!= my_AX12.isMoving()){ |
ADAMSTRUTT | 23:c1c20aee64a0 | 169 | |
wm70 | 24:462afd19d12e | 170 | my_AX12.TorqueEnable(0); |
wm70 | 24:462afd19d12e | 171 | } |
wm70 | 24:462afd19d12e | 172 | else{ |
wm70 | 24:462afd19d12e | 173 | my_AX12.TorqueEnable(1); |
ADAMSTRUTT | 23:c1c20aee64a0 | 174 | } |
wm70 | 24:462afd19d12e | 175 | |
ADAMSTRUTT | 23:c1c20aee64a0 | 176 | } |
wm70 | 24:462afd19d12e | 177 |