File content as of revision 27:d032d8fd4b45:

//*********************************************************************
// Author: 
// File: 
// Version: 
// Date: 
//*********************************************************************

#include <string>
#include "PC_Comms.h"
#include "mbed.h"
#include "MCP23017.h"
#include "WattBob_TextLCD.h"
#include "AX12.h"
    
    
    MCP23017 *par_port;
    WattBob_TextLCD *lcd;
    Serial ax12_bus(p28, p27);
    Serial _PCbus(USBTX, USBRX);
    AX12 my_AX12(ax12_bus, _PCbus, p29, 1);
    AX12 my_AX12_2(ax12_bus, _PCbus, p29, 2);
    AX12 my_AX12_3(ax12_bus, _PCbus, p29, 3);
    
    int angle3;
    char _servo;
    

//*********************************************************************
// Object: 
// Description:  
//*********************************************************************

PC_Comms::PC_Comms() 
    {
        
        par_port = new MCP23017(p9, p10, 0x40);
        par_port->config(0x0F00, 0x0F00, 0x0F00);
        lcd = new WattBob_TextLCD(par_port);
        par_port->write_bit(1, BL_BIT);
}

//*********************************************************************
// Method: PC_Connect
// Description: This method is used to connect the PC to the mbed.
//*********************************************************************

void PC_Comms::PC_Connect(void){
    
    my_AX12.SetMode(0);
    lcd->printf("Wheel Mode");
    
    wait(1);
    
    lcd->cls();
    lcd->printf("Connecting");
    lcd->locate(1,0);
    lcd->printf("to PC");
    while('s' != _PCbus.getc()){
        }
    while('s' != _PCbus.getc()){
        }
    lcd->cls();
    lcd->printf("Connected\n");
    wait(1);
}

void PC_Comms::PC_Read(void)
{
    
    char Command[8];
    
    lcd->cls();
    lcd->printf("Reading PC");
    
    if(_PCbus.readable()==1){
    Command[0] = _PCbus.getc();
    if(Command[0]=='s')
    {
        while(_PCbus.readable()==0){}
        Command[1] = _PCbus.getc();
        if(Command[1]=='c')
        {
            for(int x=2;x<=7;x++)
            {
                while(_PCbus.readable()==0){}
                Command[x] = _PCbus.getc();
            }
            
            switch(Command[2])
            {
                case '0':
                    char angle[3];
                    angle[0] = Command[4];
                    angle[1] = Command[5];
                    angle[2] = Command[6];
                    angle3 = atoi(angle);                 
                    my_AX12.SetGoal(angle3, '00');
                    break;
                
                case '1':
                    char angle1[3];
                    angle1[0] = Command[4];
                    angle1[1] = Command[5];
                    angle1[2] = Command[6];
                    angle3 = atoi(angle1);                
                    my_AX12_2.SetGoal(angle3, 'ff');
                    break;
                
                case '2':
                    char angle2[3];
                    angle2[0] = Command[4];
                    angle2[1] = Command[5];
                    angle2[2] = Command[6];
                    angle3 = atoi(angle2);                
                    my_AX12_2.SetGoal(angle3, 'ff');
                    break;
                default:
                    break;
            }
        }
    }
}
}

void PC_Comms::PC_WriteLoad(char _servo){

    char _returnToPC[4];
    int load_data;
    
    load_data = my_AX12.GetLoad();
    _returnToPC[0] = 'S';
    _returnToPC[1] = 'D';
    _returnToPC[2] = '0';
    _returnToPC[3] = '1';
    
    if(load_data >= 1000){
    _PCbus.printf("%s%d\n",_returnToPC, load_data);
    }else if(load_data < 1000 && load_data >= 100){
        _PCbus.printf("%s0%d\n", _returnToPC, load_data);
    }else if(load_data < 100 && load_data >= 10){
        _PCbus.printf("%s00%d\n", _returnToPC, load_data);
    }else if(load_data < 10){
        _PCbus.printf("%s000%d\n", _returnToPC, load_data);
    }
}
 
 void PC_Comms::PC_WriteLoad_2(char _servo){   
       
    char _returnToPC_1[4];
    int load_data_1;
    
    load_data_1 = my_AX12_2.GetLoad();
    _returnToPC_1[0] = 'S';
    _returnToPC_1[1] = 'D';
    _returnToPC_1[2] = '0';
    _returnToPC_1[3] = '2';
    
    if(load_data_1 >= 1000){
    _PCbus.printf("%s%d\n",_returnToPC_1, load_data_1);
    }else if(load_data_1 < 1000 && load_data_1 >= 100){
        _PCbus.printf("%s0%d\n", _returnToPC_1, load_data_1);
    }else if(load_data_1 < 100 && load_data_1 >= 10){
        _PCbus.printf("%s00%d\n", _returnToPC_1, load_data_1);
    }else if(load_data_1 < 10){
        _PCbus.printf("%s000%d\n", _returnToPC_1, load_data_1);
    }
}

void PC_Comms::PC_WritePosition(char _servo){
    
    char _returnToPC[4];
    int pos_data;

       pos_data = my_AX12.GetPosition();
    _returnToPC[0] = 'S';
    _returnToPC[1] = 'D';
    _returnToPC[2] = '1';
    _returnToPC[3] = _servo;
    
     
     if(pos_data >= 1000){
    _PCbus.printf("%s%i\n",_returnToPC,pos_data);
    }else if(pos_data < 1000 && pos_data >= 100){
        _PCbus.printf("%s0%i\n", _returnToPC, pos_data);
    }else if(pos_data < 100 && pos_data >= 10){
        _PCbus.printf("%s00%i\n", _returnToPC, pos_data);
    }else{
        _PCbus.printf("%s000%i\n", _returnToPC, pos_data);
    }
}

void PC_Comms::PC_WriteVoltage(char _servo){
    
    char _returnToPC[4];
    int volts_data;

    volts_data = my_AX12.GetVoltage();
    _returnToPC[0] = 'S';
    _returnToPC[1] = 'D';
    _returnToPC[2] = '1';
    _returnToPC[3] = _servo;
    
     
     if(volts_data >= 1000){
    _PCbus.printf("%s%i\n",_returnToPC,volts_data);
    }else if(volts_data < 1000 && volts_data >= 100){
        _PCbus.printf("%s0%i\n", _returnToPC, volts_data);
    }else if(volts_data < 100 && volts_data >= 10){
        _PCbus.printf("%s00%i\n", _returnToPC, volts_data);
    }else{
        _PCbus.printf("%s000%i\n", _returnToPC, volts_data);
    }
}

void PC_Comms::PC_Human(){
        
    int load_data;
    
    load_data = my_AX12.GetLoad();
    
    if(load_data >= 1070 && 1!= my_AX12.isMoving()){      

        my_AX12.TorqueEnable(0);
}       
    else{
        my_AX12.TorqueEnable(1);
}

}