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@106:1773bf7b95c5, 2015-10-26 (annotated)

Committer:
annesteenbeek
Date:
Mon Oct 26 12:46:29 2015 +0100
Revision:
106:1773bf7b95c5
Parent:
99:7030e9790b1d
Child:
107:de47331612d9
before implementation of servo without PWM

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