PC_Comms.cpp

00001 //*********************************************************************
00002 // Author: 
00003 // File: 
00004 // Version: 
00005 // Date: 
00006 //*********************************************************************
00007 
00008 #include <string>
00009 #include "PC_Comms.h"
00010 #include "mbed.h"
00011 #include "MCP23017.h"
00012 #include "WattBob_TextLCD.h"
00013 #include "AX12.h"
00014     
00015     
00016     MCP23017 *par_port;
00017     WattBob_TextLCD *lcd;
00018     Serial ax12_bus(p28, p27);
00019     Serial _PCbus(USBTX, USBRX);
00020     AX12 my_AX12(ax12_bus, _PCbus, p29, 1);
00021     AX12 my_AX12_2(ax12_bus, _PCbus, p29, 2);
00022     AX12 my_AX12_3(ax12_bus, _PCbus, p29, 3);
00023     
00024     int angle3;
00025     char _servo;
00026     
00027 
00028 //*********************************************************************
00029 // Object: 
00030 // Description:  
00031 //*********************************************************************
00032 
00033 PC_Comms::PC_Comms() 
00034     {
00035         
00036         par_port = new MCP23017(p9, p10, 0x40);
00037         par_port->config(0x0F00, 0x0F00, 0x0F00);
00038         lcd = new WattBob_TextLCD(par_port);
00039         par_port->write_bit(1, BL_BIT);
00040 }
00041 
00042 //*********************************************************************
00043 // Method: PC_Connect
00044 // Description: This method is used to connect the PC to the mbed.
00045 //*********************************************************************
00046 
00047 void PC_Comms::PC_Connect(void){
00048     
00049     my_AX12.SetMode(0);
00050     lcd->printf("Wheel Mode");
00051     
00052     wait(1);
00053     
00054     lcd->cls();
00055     lcd->printf("Connecting");
00056     lcd->locate(1,0);
00057     lcd->printf("to PC");
00058     while('s' != _PCbus.getc()){
00059         }
00060     while('s' != _PCbus.getc()){
00061         }
00062     lcd->cls();
00063     lcd->printf("Connected\n");
00064     wait(1);
00065 }
00066 
00067 void PC_Comms::PC_Read(void)
00068 {
00069     
00070     char Command[8];
00071     
00072     lcd->cls();
00073     lcd->printf("Reading PC");
00074     
00075     if(_PCbus.readable()==1){
00076     Command[0] = _PCbus.getc();
00077     if(Command[0]=='s')
00078     {
00079         while(_PCbus.readable()==0){}
00080         Command[1] = _PCbus.getc();
00081         if(Command[1]=='c')
00082         {
00083             for(int x=2;x<=7;x++)
00084             {
00085                 while(_PCbus.readable()==0){}
00086                 Command[x] = _PCbus.getc();
00087             }
00088             
00089             switch(Command[2])
00090             {
00091                 case '0':
00092                     char angle[3];
00093                     angle[0] = Command[4];
00094                     angle[1] = Command[5];
00095                     angle[2] = Command[6];
00096                     angle3 = atoi(angle);                 
00097                     my_AX12.SetGoal(angle3, '00');
00098                     break;
00099                 
00100                 case '1':
00101                     char angle1[3];
00102                     angle1[0] = Command[4];
00103                     angle1[1] = Command[5];
00104                     angle1[2] = Command[6];
00105                     angle3 = atoi(angle1);                
00106                     my_AX12_2.SetGoal(angle3, 'ff');
00107                     break;
00108                 
00109                 case '2':
00110                     char angle2[3];
00111                     angle2[0] = Command[4];
00112                     angle2[1] = Command[5];
00113                     angle2[2] = Command[6];
00114                     angle3 = atoi(angle2);                
00115                     my_AX12_2.SetGoal(angle3, 'ff');
00116                     break;
00117                 default:
00118                     break;
00119             }
00120         }
00121     }
00122 }
00123 }
00124 
00125 void PC_Comms::PC_WriteLoad(char _servo){
00126 
00127     char _returnToPC[4];
00128     int load_data;
00129     
00130     load_data = my_AX12.GetLoad();
00131     _returnToPC[0] = 'S';
00132     _returnToPC[1] = 'D';
00133     _returnToPC[2] = '0';
00134     _returnToPC[3] = '1';
00135     
00136     if(load_data >= 1000){
00137     _PCbus.printf("%s%d\n",_returnToPC, load_data);
00138     }else if(load_data < 1000 && load_data >= 100){
00139         _PCbus.printf("%s0%d\n", _returnToPC, load_data);
00140     }else if(load_data < 100 && load_data >= 10){
00141         _PCbus.printf("%s00%d\n", _returnToPC, load_data);
00142     }else if(load_data < 10){
00143         _PCbus.printf("%s000%d\n", _returnToPC, load_data);
00144     }
00145 }
00146  
00147  void PC_Comms::PC_WriteLoad_2(char _servo){   
00148        
00149     char _returnToPC_1[4];
00150     int load_data_1;
00151     
00152     load_data_1 = my_AX12_2.GetLoad();
00153     _returnToPC_1[0] = 'S';
00154     _returnToPC_1[1] = 'D';
00155     _returnToPC_1[2] = '0';
00156     _returnToPC_1[3] = '2';
00157     
00158     if(load_data_1 >= 1000){
00159     _PCbus.printf("%s%d\n",_returnToPC_1, load_data_1);
00160     }else if(load_data_1 < 1000 && load_data_1 >= 100){
00161         _PCbus.printf("%s0%d\n", _returnToPC_1, load_data_1);
00162     }else if(load_data_1 < 100 && load_data_1 >= 10){
00163         _PCbus.printf("%s00%d\n", _returnToPC_1, load_data_1);
00164     }else if(load_data_1 < 10){
00165         _PCbus.printf("%s000%d\n", _returnToPC_1, load_data_1);
00166     }
00167 }
00168 
00169 void PC_Comms::PC_WritePosition(char _servo){
00170     
00171     char _returnToPC[4];
00172     int pos_data;
00173 
00174        pos_data = my_AX12.GetPosition();
00175     _returnToPC[0] = 'S';
00176     _returnToPC[1] = 'D';
00177     _returnToPC[2] = '1';
00178     _returnToPC[3] = _servo;
00179     
00180      
00181      if(pos_data >= 1000){
00182     _PCbus.printf("%s%i\n",_returnToPC,pos_data);
00183     }else if(pos_data < 1000 && pos_data >= 100){
00184         _PCbus.printf("%s0%i\n", _returnToPC, pos_data);
00185     }else if(pos_data < 100 && pos_data >= 10){
00186         _PCbus.printf("%s00%i\n", _returnToPC, pos_data);
00187     }else{
00188         _PCbus.printf("%s000%i\n", _returnToPC, pos_data);
00189     }
00190 }
00191 
00192 void PC_Comms::PC_WriteVoltage(char _servo){
00193     
00194     char _returnToPC[4];
00195     int volts_data;
00196 
00197     volts_data = my_AX12.GetVoltage();
00198     _returnToPC[0] = 'S';
00199     _returnToPC[1] = 'D';
00200     _returnToPC[2] = '1';
00201     _returnToPC[3] = _servo;
00202     
00203      
00204      if(volts_data >= 1000){
00205     _PCbus.printf("%s%i\n",_returnToPC,volts_data);
00206     }else if(volts_data < 1000 && volts_data >= 100){
00207         _PCbus.printf("%s0%i\n", _returnToPC, volts_data);
00208     }else if(volts_data < 100 && volts_data >= 10){
00209         _PCbus.printf("%s00%i\n", _returnToPC, volts_data);
00210     }else{
00211         _PCbus.printf("%s000%i\n", _returnToPC, volts_data);
00212     }
00213 }
00214 
00215 void PC_Comms::PC_Human(){
00216         
00217     int load_data;
00218     
00219     load_data = my_AX12.GetLoad();
00220     
00221     if(load_data >= 1070 && 1!= my_AX12.isMoving()){      
00222 
00223         my_AX12.TorqueEnable(0);
00224 }       
00225     else{
00226         my_AX12.TorqueEnable(1);
00227 }
00228 
00229 }
00230