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@130:2542f844ba1e, 2015-10-30 (annotated)

Committer:
annesteenbeek
Date:
Fri Oct 30 10:32:49 2015 +0100
Revision:
130:2542f844ba1e
Parent:
124:f67ce69557db 
changes

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