William McColl / Mbed 2 deprecated Balanced-Arm

Motor control for robotic arm

Dependencies:   MCP23017 WattBob_TextLCD mbed

Fork of Balanced Arm by Balanced Arm

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?

UserRevisionLine numberNew 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