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@24:462afd19d12e, 2016-03-03 (annotated)

Committer:
wm70
Date:
Thu Mar 03 12:07:25 2016 +0000
Revision:
24:462afd19d12e
Parent:
23:c1c20aee64a0
Child:
25:d345ef4cb2b5
Position - yes

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