Dammenisvoorcasuals / Mbed 2 deprecated chessRobot

control for robotic arm that can play chess using a granular gripper

Dependencies:   Encoder mbed HIDScope Servo MODSERIAL

Fork of chessRobot by a steenbeek

buttons.cpp@102:b3ab436fbe8e, 2015-10-22 (annotated)

Committer:
annesteenbeek
Date:
Thu Oct 22 10:12:27 2015 +0000
Revision:
102:b3ab436fbe8e
Parent:
101:b821e89fc108
Child:
103:4a37d19e8fcc
working EMG and motor1 encoder pins switched;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
annesteenbeek 14:0c0d1bfd94ea 1 #include "buttons.h"
annesteenbeek 14:0c0d1bfd94ea 2 #include "mbed.h"
annesteenbeek 17:872fcf85116d 3 #include "config.h"
annesteenbeek 25:874675516927 4 #include "actuators.h"
annesteenbeek 57:43f707648f2b 5 #include "PID.h"
annesteenbeek 57:43f707648f2b 6
annesteenbeek 101:b821e89fc108 7 //#define TUNEPID // enable tuning PID parameters with potmeters
annesteenbeek 101:b821e89fc108 8 //#define SETPOS // enable position tuning with potmeters
annesteenbeek 0:525558a26464 9 // functions for reading all the buttons and switches
annesteenbeek 27:5eb5ec295ab2 10 AnalogIn pot2(pot2Pin);
annesteenbeek 30:a20f16bf8dda 11 AnalogIn pot1(pot1Pin);
annesteenbeek 0:525558a26464 12
annesteenbeek 32:2006977785f5 13 // Led states:
annesteenbeek 32:2006977785f5 14 // 0 leds: motors disabled
annesteenbeek 35:e26a573e3e9f 15 // redLed: control X speed
annesteenbeek 35:e26a573e3e9f 16 // greenLed: control Y speed
annesteenbeek 35:e26a573e3e9f 17 // blueLed: control Servo pos
annesteenbeek 36:6f9670eb9168 18 DigitalOut redLed(LED_RED);
annesteenbeek 36:6f9670eb9168 19 DigitalOut greenLed(LED_GREEN);
annesteenbeek 36:6f9670eb9168 20 DigitalOut blueLed(LED_BLUE);
annesteenbeek 32:2006977785f5 21
annesteenbeek 41:d5c3055a7bc7 22 DigitalIn button1(button1Pin);
annesteenbeek 41:d5c3055a7bc7 23 DigitalIn button2(button2Pin);
annesteenbeek 42:d36d216457c4 24 bool button1Pressed = false;
annesteenbeek 42:d36d216457c4 25 bool button2Pressed = false;
annesteenbeek 32:2006977785f5 26 int actuatorState = 0;
annesteenbeek 57:43f707648f2b 27 int PIDparam = 0;
annesteenbeek 32:2006977785f5 28
annesteenbeek 62:6c566e6f9664 29 double pot1Val = 0;
annesteenbeek 62:6c566e6f9664 30 double pot2Val = 0;
annesteenbeek 62:6c566e6f9664 31 double Kp = 1;
annesteenbeek 62:6c566e6f9664 32 double Ki = 0;
annesteenbeek 62:6c566e6f9664 33 double Kd = 0;
annesteenbeek 34:f315b2b38555 34
annesteenbeek 32:2006977785f5 35
annesteenbeek 0:525558a26464 36 void checkSwitches(){
annesteenbeek 32:2006977785f5 37
annesteenbeek 57:43f707648f2b 38 // read potmeter values
annesteenbeek 57:43f707648f2b 39 pot1Val = pot1.read();
annesteenbeek 57:43f707648f2b 40 pot2Val = pot2.read();
annesteenbeek 32:2006977785f5 41
annesteenbeek 57:43f707648f2b 42 // button 1 pressed
annesteenbeek 57:43f707648f2b 43 if(button1.read() == 0){
annesteenbeek 40:0d88aa25a57d 44 if (button1Pressed != true){
annesteenbeek 40:0d88aa25a57d 45 motorsEnable = !motorsEnable;
annesteenbeek 40:0d88aa25a57d 46 button1Pressed = true;
annesteenbeek 40:0d88aa25a57d 47 }
annesteenbeek 40:0d88aa25a57d 48 }else{
annesteenbeek 40:0d88aa25a57d 49 button1Pressed = false;
annesteenbeek 32:2006977785f5 50 }
annesteenbeek 57:43f707648f2b 51
annesteenbeek 101:b821e89fc108 52 // #ifdef TUNEPID
annesteenbeek 101:b821e89fc108 53 // setForPID(); // if TUNEPID is defined, use potmeters for PID tuning
annesteenbeek 101:b821e89fc108 54 // #endif
annesteenbeek 101:b821e89fc108 55 #ifdef SETPOS
annesteenbeek 57:43f707648f2b 56 setForPositions(); // else use it for motor control
annesteenbeek 101:b821e89fc108 57 #else // still show that motors are enabled using RGB led
annesteenbeek 101:b821e89fc108 58 if(motorsEnable){
annesteenbeek 101:b821e89fc108 59 redLed.write(0); greenLed.write(0); blueLed.write(1);
annesteenbeek 101:b821e89fc108 60 }else{
annesteenbeek 101:b821e89fc108 61 redLed.write(1); greenLed.write(1); blueLed.write(1);
annesteenbeek 101:b821e89fc108 62 }
annesteenbeek 57:43f707648f2b 63 #endif
annesteenbeek 57:43f707648f2b 64 }
annesteenbeek 57:43f707648f2b 65
annesteenbeek 57:43f707648f2b 66 void setForPID(){
annesteenbeek 57:43f707648f2b 67 if(button2.read() == 0){
annesteenbeek 57:43f707648f2b 68 if(button2Pressed != true){
annesteenbeek 57:43f707648f2b 69 PIDparam++;
annesteenbeek 57:43f707648f2b 70 if(PIDparam==3){
annesteenbeek 57:43f707648f2b 71 PIDparam = 0;
annesteenbeek 57:43f707648f2b 72 }
annesteenbeek 57:43f707648f2b 73 button2Pressed = true;
annesteenbeek 57:43f707648f2b 74 }
annesteenbeek 57:43f707648f2b 75 }else{
annesteenbeek 57:43f707648f2b 76 button2Pressed = false;
annesteenbeek 57:43f707648f2b 77 }
annesteenbeek 57:43f707648f2b 78
annesteenbeek 57:43f707648f2b 79 if(motorsEnable){
annesteenbeek 57:43f707648f2b 80 motor2SetSpeed = 300*pot2.read();
annesteenbeek 57:43f707648f2b 81 switch (PIDparam){
annesteenbeek 57:43f707648f2b 82 case 0: // potmeters control P gain
annesteenbeek 57:43f707648f2b 83 redLed.write(0); greenLed.write(1); blueLed.write(1);
annesteenbeek 60:20945383ad1b 84 Kp =3* pot1.read()+1;
annesteenbeek 57:43f707648f2b 85 break;
annesteenbeek 57:43f707648f2b 86 case 1: // potmeters control I gain
annesteenbeek 57:43f707648f2b 87 redLed.write(1); greenLed.write(0); blueLed.write(1);
annesteenbeek 60:20945383ad1b 88 Ki = pot1.read();
annesteenbeek 57:43f707648f2b 89 break;
annesteenbeek 57:43f707648f2b 90 case 2: // potmeters control D gain
annesteenbeek 57:43f707648f2b 91 redLed.write(1); greenLed.write(1); blueLed.write(0);
annesteenbeek 60:20945383ad1b 92 Kd = pot1.read();
annesteenbeek 57:43f707648f2b 93 break;
annesteenbeek 57:43f707648f2b 94 }
annesteenbeek 57:43f707648f2b 95 motor2PID.SetTunings(Kp, Ki, Kd);
annesteenbeek 57:43f707648f2b 96 }else{
annesteenbeek 57:43f707648f2b 97 redLed.write(1); greenLed.write(1); blueLed.write(1);
annesteenbeek 57:43f707648f2b 98 }
annesteenbeek 57:43f707648f2b 99 }
annesteenbeek 57:43f707648f2b 100
annesteenbeek 57:43f707648f2b 101 void setForPositions(){
annesteenbeek 57:43f707648f2b 102 // 3 states: X control, Y control en Servo control
annesteenbeek 57:43f707648f2b 103 // button 1 to enable/disable actuators
annesteenbeek 57:43f707648f2b 104 // button 2 to switch between states
annesteenbeek 57:43f707648f2b 105
annesteenbeek 57:43f707648f2b 106
annesteenbeek 44:a947bc232d84 107 if(button2.read() == 0){
annesteenbeek 40:0d88aa25a57d 108 if(button2Pressed != true){
annesteenbeek 40:0d88aa25a57d 109 actuatorState++;
annesteenbeek 40:0d88aa25a57d 110 if(actuatorState==3){
annesteenbeek 40:0d88aa25a57d 111 actuatorState = 0;
annesteenbeek 40:0d88aa25a57d 112 }
annesteenbeek 40:0d88aa25a57d 113 button2Pressed = true;
annesteenbeek 34:f315b2b38555 114 }
annesteenbeek 40:0d88aa25a57d 115 }else{
annesteenbeek 40:0d88aa25a57d 116 button2Pressed = false;
annesteenbeek 32:2006977785f5 117 }
annesteenbeek 32:2006977785f5 118
annesteenbeek 57:43f707648f2b 119
annesteenbeek 32:2006977785f5 120
annesteenbeek 32:2006977785f5 121 if(motorsEnable){
annesteenbeek 32:2006977785f5 122 switch (actuatorState){
annesteenbeek 32:2006977785f5 123 case 0: // potmeters control X speed
annesteenbeek 36:6f9670eb9168 124 redLed.write(0); greenLed.write(1); blueLed.write(1);
annesteenbeek 102:b3ab436fbe8e 125 motor1SetSpeed = scaleXSpeed*(pot2.read()-pot1.read());
annesteenbeek 32:2006977785f5 126 break;
annesteenbeek 32:2006977785f5 127 case 1: // potmeters control Y speed
annesteenbeek 36:6f9670eb9168 128 redLed.write(1); greenLed.write(0); blueLed.write(1);
annesteenbeek 102:b3ab436fbe8e 129 motor2SetSpeed = scaleYSpeed*(pot2.read()-pot1.read());
annesteenbeek 32:2006977785f5 130 break;
annesteenbeek 32:2006977785f5 131 case 2: // potmeters control Servo pos
annesteenbeek 36:6f9670eb9168 132 redLed.write(1); greenLed.write(1); blueLed.write(0);
annesteenbeek 102:b3ab436fbe8e 133 servoSpeed = scaleZSpeed*(pot2.read()-pot1.read());
annesteenbeek 32:2006977785f5 134 break;
annesteenbeek 32:2006977785f5 135 }
annesteenbeek 32:2006977785f5 136 }else{
annesteenbeek 36:6f9670eb9168 137 redLed.write(1); greenLed.write(1); blueLed.write(1);
annesteenbeek 32:2006977785f5 138 }
annesteenbeek 57:43f707648f2b 139 }