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@107:de47331612d9, 2015-10-26 (annotated)

Committer:
annesteenbeek
Date:
Mon Oct 26 13:04:22 2015 +0100
Revision:
107:de47331612d9
Parent:
106:1773bf7b95c5
Child:
118:49605b5bd802
added servo control 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 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 57:43f707648f2b 57 setForPositions(); // else use it for motor control
annesteenbeek 57:43f707648f2b 58 #endif
annesteenbeek 57:43f707648f2b 59 }
annesteenbeek 57:43f707648f2b 60
annesteenbeek 57:43f707648f2b 61 void setForPID(){
annesteenbeek 57:43f707648f2b 62 if(button2.read() == 0){
annesteenbeek 57:43f707648f2b 63 if(button2Pressed != true){
annesteenbeek 57:43f707648f2b 64 PIDparam++;
annesteenbeek 57:43f707648f2b 65 if(PIDparam==3){
annesteenbeek 57:43f707648f2b 66 PIDparam = 0;
annesteenbeek 57:43f707648f2b 67 }
annesteenbeek 57:43f707648f2b 68 button2Pressed = true;
annesteenbeek 57:43f707648f2b 69 }
annesteenbeek 57:43f707648f2b 70 }else{
annesteenbeek 57:43f707648f2b 71 button2Pressed = false;
annesteenbeek 57:43f707648f2b 72 }
annesteenbeek 57:43f707648f2b 73
annesteenbeek 57:43f707648f2b 74 if(motorsEnable){
annesteenbeek 57:43f707648f2b 75 motor2SetSpeed = 300*pot2.read();
annesteenbeek 57:43f707648f2b 76 switch (PIDparam){
annesteenbeek 57:43f707648f2b 77 case 0: // potmeters control P gain
annesteenbeek 57:43f707648f2b 78 redLed.write(0); greenLed.write(1); blueLed.write(1);
annesteenbeek 60:20945383ad1b 79 Kp =3* pot1.read()+1;
annesteenbeek 57:43f707648f2b 80 break;
annesteenbeek 57:43f707648f2b 81 case 1: // potmeters control I gain
annesteenbeek 57:43f707648f2b 82 redLed.write(1); greenLed.write(0); blueLed.write(1);
annesteenbeek 60:20945383ad1b 83 Ki = pot1.read();
annesteenbeek 57:43f707648f2b 84 break;
annesteenbeek 57:43f707648f2b 85 case 2: // potmeters control D gain
annesteenbeek 57:43f707648f2b 86 redLed.write(1); greenLed.write(1); blueLed.write(0);
annesteenbeek 60:20945383ad1b 87 Kd = pot1.read();
annesteenbeek 57:43f707648f2b 88 break;
annesteenbeek 57:43f707648f2b 89 }
annesteenbeek 57:43f707648f2b 90 motor2PID.SetTunings(Kp, Ki, Kd);
annesteenbeek 57:43f707648f2b 91 }else{
annesteenbeek 57:43f707648f2b 92 redLed.write(1); greenLed.write(1); blueLed.write(1);
annesteenbeek 57:43f707648f2b 93 }
annesteenbeek 57:43f707648f2b 94 }
annesteenbeek 57:43f707648f2b 95
annesteenbeek 57:43f707648f2b 96 void setForPositions(){
annesteenbeek 57:43f707648f2b 97 // 3 states: X control, Y control en Servo control
annesteenbeek 57:43f707648f2b 98 // button 1 to enable/disable actuators
annesteenbeek 57:43f707648f2b 99 // button 2 to switch between states
annesteenbeek 57:43f707648f2b 100
annesteenbeek 57:43f707648f2b 101
annesteenbeek 44:a947bc232d84 102 if(button2.read() == 0){
annesteenbeek 40:0d88aa25a57d 103 if(button2Pressed != true){
annesteenbeek 40:0d88aa25a57d 104 actuatorState++;
annesteenbeek 40:0d88aa25a57d 105 if(actuatorState==3){
annesteenbeek 40:0d88aa25a57d 106 actuatorState = 0;
annesteenbeek 40:0d88aa25a57d 107 }
annesteenbeek 40:0d88aa25a57d 108 button2Pressed = true;
annesteenbeek 34:f315b2b38555 109 }
annesteenbeek 40:0d88aa25a57d 110 }else{
annesteenbeek 40:0d88aa25a57d 111 button2Pressed = false;
annesteenbeek 32:2006977785f5 112 }
annesteenbeek 32:2006977785f5 113
annesteenbeek 57:43f707648f2b 114
annesteenbeek 32:2006977785f5 115
annesteenbeek 32:2006977785f5 116 if(motorsEnable){
annesteenbeek 32:2006977785f5 117 switch (actuatorState){
annesteenbeek 32:2006977785f5 118 case 0: // potmeters control X speed
annesteenbeek 36:6f9670eb9168 119 redLed.write(0); greenLed.write(1); blueLed.write(1);
annesteenbeek 54:c14c3bc48b8a 120 motor1SetSpeed = 300*(pot2.read()-pot1.read());
annesteenbeek 32:2006977785f5 121 break;
annesteenbeek 32:2006977785f5 122 case 1: // potmeters control Y speed
annesteenbeek 36:6f9670eb9168 123 redLed.write(1); greenLed.write(0); blueLed.write(1);
annesteenbeek 54:c14c3bc48b8a 124 motor2SetSpeed = 300*(pot2.read()-pot1.read());
annesteenbeek 32:2006977785f5 125 break;
annesteenbeek 32:2006977785f5 126 case 2: // potmeters control Servo pos
annesteenbeek 36:6f9670eb9168 127 redLed.write(1); greenLed.write(1); blueLed.write(0);
annesteenbeek 99:7030e9790b1d 128 servoSpeed = pot2.read();
annesteenbeek 32:2006977785f5 129 break;
annesteenbeek 32:2006977785f5 130 }
annesteenbeek 32:2006977785f5 131 }else{
annesteenbeek 36:6f9670eb9168 132 redLed.write(1); greenLed.write(1); blueLed.write(1);
annesteenbeek 32:2006977785f5 133 }
annesteenbeek 57:43f707648f2b 134 }