William McColl / Mbed 2 deprecated Balanced-Arm

Motor control for robotic arm

Dependencies:   MCP23017 WattBob_TextLCD mbed

Fork of Balanced Arm by Balanced Arm

HandShake/HandShake.cpp@11:7f74fb17c328, 2016-02-01 (annotated)

Committer:
ADAMSTRUTT
Date:
Mon Feb 01 15:48:29 2016 +0000
Revision:
11:7f74fb17c328
Parent:
10:d7cce35b7bfd 
Torque almost working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ADAMSTRUTT 7:40d7d88deba3 1 //*********************************************************************
ADAMSTRUTT 7:40d7d88deba3 2 // Author: Adam Strutt
ADAMSTRUTT 7:40d7d88deba3 3 // File: (PS2C.cpp) PlayStation 2 controller
ADAMSTRUTT 7:40d7d88deba3 4 // Version: 1.0v
ADAMSTRUTT 7:40d7d88deba3 5 // Date: 11/12/2015
ADAMSTRUTT 7:40d7d88deba3 6 //*********************************************************************
ADAMSTRUTT 7:40d7d88deba3 7
ADAMSTRUTT 7:40d7d88deba3 8 #include <string>
ADAMSTRUTT 7:40d7d88deba3 9 #include "HandShake.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);
ADAMSTRUTT 11:7f74fb17c328 18 AX12 my_AX12(ax12_bus, p29, 1);
ADAMSTRUTT 7:40d7d88deba3 19
ADAMSTRUTT 7:40d7d88deba3 20 //*********************************************************************
ADAMSTRUTT 7:40d7d88deba3 21 // Object: PC_HandShake
ADAMSTRUTT 7:40d7d88deba3 22 // Description: To create a HandShake object.
ADAMSTRUTT 7:40d7d88deba3 23 //*********************************************************************
ADAMSTRUTT 7:40d7d88deba3 24
ADAMSTRUTT 11:7f74fb17c328 25 PC_HandShake::PC_HandShake(Serial& PCbus)
ADAMSTRUTT 11:7f74fb17c328 26 : _PCbus(PCbus){
ADAMSTRUTT 7:40d7d88deba3 27
ADAMSTRUTT 11:7f74fb17c328 28 par_port = new MCP23017(p9, p10, 0x40);
ADAMSTRUTT 11:7f74fb17c328 29 par_port->config(0x0F00, 0x0F00, 0x0F00);
ADAMSTRUTT 11:7f74fb17c328 30 lcd = new WattBob_TextLCD(par_port);
ADAMSTRUTT 11:7f74fb17c328 31 par_port->write_bit(1, BL_BIT);
ADAMSTRUTT 7:40d7d88deba3 32 }
ADAMSTRUTT 7:40d7d88deba3 33
ADAMSTRUTT 7:40d7d88deba3 34 //*********************************************************************
ADAMSTRUTT 7:40d7d88deba3 35 // Method: PC_Connect
ADAMSTRUTT 7:40d7d88deba3 36 // Description: This method is used to connect the PC to the mbed.
ADAMSTRUTT 7:40d7d88deba3 37 //*********************************************************************
ADAMSTRUTT 7:40d7d88deba3 38
ADAMSTRUTT 7:40d7d88deba3 39 void PC_HandShake::PC_Connect(void){
ADAMSTRUTT 11:7f74fb17c328 40 lcd->cls();
ADAMSTRUTT 11:7f74fb17c328 41 lcd->printf("Connecting");
ADAMSTRUTT 11:7f74fb17c328 42 lcd->locate(1,0);
ADAMSTRUTT 11:7f74fb17c328 43 lcd->printf("to PC");
Hamilton252 10:d7cce35b7bfd 44 while('s' != _PCbus.getc()){
ADAMSTRUTT 7:40d7d88deba3 45 }
Hamilton252 10:d7cce35b7bfd 46 while('s' != _PCbus.getc()){
ADAMSTRUTT 7:40d7d88deba3 47 }
ADAMSTRUTT 11:7f74fb17c328 48 lcd->cls();
ADAMSTRUTT 11:7f74fb17c328 49 lcd->printf("Conneced\n");
ADAMSTRUTT 7:40d7d88deba3 50 wait(2);
wm70 9:87558e7cfecb 51 }
wm70 9:87558e7cfecb 52
wm70 9:87558e7cfecb 53 void PC_HandShake::PC_Read(void){
ADAMSTRUTT 11:7f74fb17c328 54
Hamilton252 10:d7cce35b7bfd 55 char Command[8];
ADAMSTRUTT 11:7f74fb17c328 56
ADAMSTRUTT 11:7f74fb17c328 57 lcd->cls();
ADAMSTRUTT 11:7f74fb17c328 58 lcd->printf("Reading PC");
wm70 9:87558e7cfecb 59
wm70 9:87558e7cfecb 60 while(_PCbus.readable()==0){}
ADAMSTRUTT 11:7f74fb17c328 61 Command[0] = _PCbus.getc();
ADAMSTRUTT 11:7f74fb17c328 62 if(Command[0]=='s')
Hamilton252 10:d7cce35b7bfd 63 {
ADAMSTRUTT 11:7f74fb17c328 64 while(_PCbus.readable()==0){}
ADAMSTRUTT 11:7f74fb17c328 65 Command[1] = _PCbus.getc();
ADAMSTRUTT 11:7f74fb17c328 66 if(Command[1]=='c')
ADAMSTRUTT 11:7f74fb17c328 67 {
ADAMSTRUTT 11:7f74fb17c328 68 for(int x=2;x<=7;x++)
ADAMSTRUTT 11:7f74fb17c328 69 {
ADAMSTRUTT 11:7f74fb17c328 70 while(_PCbus.readable()==0){}
Hamilton252 10:d7cce35b7bfd 71 Command[x] = _PCbus.getc();
Hamilton252 10:d7cce35b7bfd 72 }
Hamilton252 10:d7cce35b7bfd 73
ADAMSTRUTT 11:7f74fb17c328 74 lcd->cls();
ADAMSTRUTT 11:7f74fb17c328 75 lcd->printf("%s", Command);
ADAMSTRUTT 11:7f74fb17c328 76 wait(1);
ADAMSTRUTT 11:7f74fb17c328 77 switch(Command[2]){
ADAMSTRUTT 11:7f74fb17c328 78 case '0':
ADAMSTRUTT 11:7f74fb17c328 79 char angle[3];
ADAMSTRUTT 11:7f74fb17c328 80 angle[0] = Command[4];
ADAMSTRUTT 11:7f74fb17c328 81 angle[1] = Command[5];
ADAMSTRUTT 11:7f74fb17c328 82 angle[2] = Command[6];
ADAMSTRUTT 11:7f74fb17c328 83 int angle2 = atoi(angle);
ADAMSTRUTT 11:7f74fb17c328 84 lcd->cls();
ADAMSTRUTT 11:7f74fb17c328 85 lcd->printf("%i", angle2);
ADAMSTRUTT 11:7f74fb17c328 86 my_AX12.SetGoal(angle2);
ADAMSTRUTT 11:7f74fb17c328 87 break;
ADAMSTRUTT 11:7f74fb17c328 88 default:
ADAMSTRUTT 11:7f74fb17c328 89 break;
ADAMSTRUTT 11:7f74fb17c328 90 }
ADAMSTRUTT 11:7f74fb17c328 91 }
ADAMSTRUTT 11:7f74fb17c328 92 wait(1);
ADAMSTRUTT 11:7f74fb17c328 93 }
ADAMSTRUTT 11:7f74fb17c328 94 }
Hamilton252 10:d7cce35b7bfd 95
ADAMSTRUTT 11:7f74fb17c328 96 void PC_HandShake::PC_WriteLoad(char _servo){
wm70 9:87558e7cfecb 97
ADAMSTRUTT 11:7f74fb17c328 98 char _data[3];
ADAMSTRUTT 11:7f74fb17c328 99 char _returnToPC[8];
ADAMSTRUTT 11:7f74fb17c328 100
ADAMSTRUTT 11:7f74fb17c328 101 lcd->cls();
ADAMSTRUTT 11:7f74fb17c328 102 lcd->printf("Writing to PC");
ADAMSTRUTT 11:7f74fb17c328 103
ADAMSTRUTT 11:7f74fb17c328 104 my_AX12.GetLoad(_data);
ADAMSTRUTT 11:7f74fb17c328 105
ADAMSTRUTT 11:7f74fb17c328 106 //lcd->cls();
ADAMSTRUTT 11:7f74fb17c328 107 // lcd->printf("%s", _data);
ADAMSTRUTT 11:7f74fb17c328 108
ADAMSTRUTT 11:7f74fb17c328 109 _returnToPC[0] = 'S';
ADAMSTRUTT 11:7f74fb17c328 110 _returnToPC[1] = 'D';
ADAMSTRUTT 11:7f74fb17c328 111 _returnToPC[2] = '0';
ADAMSTRUTT 11:7f74fb17c328 112 _returnToPC[3] = _servo;
ADAMSTRUTT 11:7f74fb17c328 113 _returnToPC[4] = _data[0];
ADAMSTRUTT 11:7f74fb17c328 114 _returnToPC[5] = _data[1];
ADAMSTRUTT 11:7f74fb17c328 115 _returnToPC[6] = _data[2];
ADAMSTRUTT 11:7f74fb17c328 116 _returnToPC[7] = '0';
ADAMSTRUTT 11:7f74fb17c328 117
ADAMSTRUTT 11:7f74fb17c328 118 _PCbus.printf(_returnToPC);
ADAMSTRUTT 11:7f74fb17c328 119 wait(1);
ADAMSTRUTT 7:40d7d88deba3 120 }