![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
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?
User | Revision | Line number | New 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 | } |