Nick van den Berg
Cleaner version
Dependencies: HIDScope MODSERIAL Motordriver QEI Servo mbed
Fork of
The_Claw_with_EMG_Control_PID
by 
Meike Froklage
Diff: main.cpp
- Revision:
- 5:3d88f7506cd9
- Parent:
- 4:e62a2df0a5b5
- Child:
- 6:23b1ed826b59
--- a/main.cpp Wed Oct 19 14:03:19 2016 +0000
+++ b/main.cpp Fri Oct 28 09:01:41 2016 +0000
@@ -1,111 +1,315 @@
#include "mbed.h"
+#include "MODSERIAL.h"
+#define SERIAL_BAUD 115200
#include "motordriver.h"
+#include "QEI.h"
+#include "Servo.h"
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
-// Serial communication using MODSERIAL
-#define SERIAL_BAUD 115200 // baud rate for serial communication
-#include "MODSERIAL.h"
+//======== Serial Communication ================================================
MODSERIAL pc(USBTX,USBRX);
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
-// Timing
-const float kTimeLedToggle = 0.25f; // period with which to toggle LED
-const float kTimePrintSerial = 1.0f;// period with which data is printed
+//======== Motor and QEI =======================================================
+int Brakeable;
+int sign;
+
+// motor
+Motor Cart(D5, D4, D4, Brakeable); // right motor
+Motor Arm(D6,D7, D7, Brakeable); // left motor
+
+// qei
+QEI Encoder_Cart(D10, D11, NC, 64);
+QEI Encoder_Arm(D12, D13, NC, 64);
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
-// constants
-const int LedOn = 0; // LED on if 0
+// servo
+Servo servo(D9);
-// LEDs
+//======== Miscellaneous =======================================================
+// button
+InterruptIn btn(SW2);
+InterruptIn btn2(SW3);
+
+InterruptIn btn_cart(D1);
+InterruptIn btn_arm(D2);
+InterruptIn btn_claw(D3);
+
+// led
DigitalOut led_r(LED_RED);
DigitalOut led_g(LED_GREEN);
DigitalOut led_b(LED_BLUE);
-// ID of led that should blink; *volatile* because changed by interrupt
-volatile int part_id = 0;
+// potmeter
+AnalogIn pot_cart(A2);
+AnalogIn pot_arm(A3);
+// ticker
+Ticker tick_part; // ticker to switch parts
+
+//======== Variables ===========================================================
// counters
-int num_turned_on_0 = 0; // count number of times red LED turned on
-int num_turned_on_1 = 0; // count number of times green LED turned on
-int num_turned_on_2 = 0; // count number of times blue LED turned on
+int num_turned_on_0 = 0; // count number of times red LED turned on
+int num_turned_on_1 = 0; // count number of times green LED turned on
+int num_turned_on_2 = 0; // count number of times blue LED turned on
+
+int num_claw_turned_on_0 = 0; // count number of times red LED turned on
+int num_claw_turned_on_1 = 0; // count number of times green LED turned on
+int num_claw_turned_on_2 = 0; // count number of times blue LED turned on
+// speed
+double cart_speed = 0.5;
+double cart_stop = 0.2;
+double arm_speed = 0.3;
+double arm_stop = 0.1;
-//Safety for Motor
-int Brakeable; //cna the motor driver break
-int sign; //prevents throwing the motor from full foward to full reverse and stuff melting.
+// position
+float factor_cart = 0.06559;
+float factor_arm = 0.1539;
+int position_cart;
+int position_arm;
+float ain_cart; //Variable to store the analog input of the cart
+float ain_arm; //Variable to store the analog input of the arm
-//Motor
-Motor A(D6, D7, D7, Brakeable); // pwm, fwd, rev, brake (right)
-Motor B(D5, D4, D4, Brakeable); // pwm, fwd, rev, brake (left)
+// miscellaneous
+const float kTimeToggle = 0.25f; // period with which to toggle the parts
+const int LedOn = 0; // LED on if 0
+volatile int part_id = 2; // ID of what part should move, begins with the cart
+volatile int servo_id = 1; // ID to the side the servo should move, begins in center position
+//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+//======== Functions and main ==============================================================
+// Switch between Cart, Arm and Claw
void SwitchPart()
{
switch (part_id) {
+ //Cart
case 2: {
- // LED 0 to toggle and count:
- led_r = LedOn;
- if (led_r == LedOn) {
+ led_r = LedOn;
+ if(led_r == LedOn){
num_turned_on_0++;
+
+ if(btn && btn2) {
+ Arm.speed(0) == 0;
+ Cart.speed(0) == 0;
+
+ }else if (btn && !btn2) {
+ if(position_cart <= -105){ //If the cart is at the right side, it stops
+ Cart.speed(-cart_stop)==-cart_stop;
+
+ }else if(position_cart >= 105 && position_arm <=-60){ //If the cart is at the left side and the arm is rotated 60 degrees to the left, the cart can't move to the right.
+ Cart.speed(cart_stop) == cart_stop;
+
+ }else{
+ Cart.speed(cart_speed)==cart_speed;
+ }
+
+ }else if (!btn && btn2) {
+ if(position_cart >= 105){ //If the cart is at the left side, it stops
+ Cart.speed(cart_stop)==cart_stop;
+
+ }else if(position_cart <= -105 && position_arm >=60){ //If the cart is at the right side and the arm is rotated 60 degrees to the right, the cart can't move to the left.
+ Cart.speed(-cart_stop) == -cart_stop;
+
+ }else{
+ Cart.speed(-cart_speed)==-cart_speed;
+ }
+
+ }else {
+ Arm.speed(0) == 0;
+ Cart.speed(0) == 0;
+ }
}
-
- // LEDs to turn off:
+ // controle LED
led_g = not LedOn;
- led_b = not LedOn;
- A.speed(0) == 0;
- B.speed(0) == 0;
+ led_b = not LedOn;
+
+ // encoder
+ position_cart = (Encoder_Cart.getPulses()*factor_cart) ;
+ ain_cart = pot_cart.read();
+
+ if (ain_cart == 0){
+ Encoder_Cart.reset();
+ }else {}
+
+ wait(0.1);
+ pc.baud(115200);
+ pc.printf("Distance in mm: %i\n", position_cart);
+
break;
}
+
+ //Arm
case 3: {
- // LED 1 to toggle and count:
- led_g = LedOn;
- A.speed(0.5) == 0.5;
- if (led_g == LedOn) {
+ led_g = LedOn;
+ if(led_g == LedOn){
num_turned_on_1++;
- }
+
+ if(btn && btn2) {
+ Arm.speed(0) == 0;
+ Cart.speed(0) == 0;
+ }
- // LEDs to turn off:
+ else if (btn && !btn2) {
+ if(position_cart > -105 && position_arm >= 30){ //If the cart is not at the end, the arm can't move any further than 30 degrees
+ Arm.speed(-arm_stop)==-arm_stop;
+
+ }else if(position_cart<= -105 && position_arm>=80){ //If the cart is at the right end, the arm can't move any further than 70 degrees
+ Arm.speed(-arm_stop)==-arm_stop;
+
+ }else{
+ Arm.speed(arm_speed)==arm_speed;
+ }
+
+ }else if (!btn && btn2) {
+ if(position_cart < 105 && position_arm <= -30){ //If the cart is not at the end, the arm can't move any further than 30 degrees
+ Arm.speed(arm_stop)==arm_stop;
+
+ }else if(position_cart>=105 && position_arm<=-80){ //If the cart is at the left end, the arm can't move any further than 70 degrees
+ Arm.speed(arm_stop)==arm_stop;
+
+ }else{
+ Arm.speed(-arm_speed)==-arm_speed;
+ }
+
+ }else {
+ Arm.speed(0) == 0;
+ Cart.speed(0) == 0;
+ }
+ }
+ // controle LED
led_r = not LedOn;
- led_b = not LedOn;
- B.speed(0) == 0;
-
+ led_b = not LedOn;
+
+ // encoder
+ position_arm = (Encoder_Arm.getPulses()*factor_arm) ;
+ ain_arm = pot_arm.read();
+
+ if (ain_arm == 0){
+ Encoder_Arm.reset();
+ }else {}
+
+ wait(0.1);
+ pc.baud(115200);
+ pc.printf("Degrees: %i\n", position_arm);
+
+
break;
}
+
+ //Claw
case 4: {
- // LED 2 to toggle and count:
led_b = LedOn;
- B.speed(0.5) == 0.5;
- if (led_b == LedOn) {
+ if(led_b == LedOn){
num_turned_on_2++;
+
+ if(btn && btn2){
+
+ }else if(btn && !btn2){
+ servo_id ++;
+
+ switch (servo_id) {
+ case 0: {
+ led_r = LedOn;
+ if (led_r == LedOn) {
+ num_claw_turned_on_0++;
+ }
+
+ led_b = not LedOn;
+ led_g = not LedOn;
+
+ servo.position(27);
+ pc.printf("Servo position is: left \r\n");
+ break;
+ }
+ case 1: {
+ led_b = LedOn;
+ if (led_b == LedOn) {
+ num_claw_turned_on_1++;
+ }
+
+ led_r = not LedOn;
+ led_g = not LedOn;
+
+ servo.position(3);
+ pc.printf("Servo position is: center \r\n");
+ break;
+ }
+ case 2: {
+ led_g = LedOn;
+ if (led_g == LedOn) {
+ num_claw_turned_on_2++;
+ }
+
+ led_r = not LedOn;
+ led_b = not LedOn;
+
+ servo.position(-18);
+ pc.printf("Servo position is: right \r\n");
+ break;
+ }
+ }
+
+
+ }else if(!btn && btn2){
+ servo_id --;
+
+ switch (servo_id) {
+ case 0: {
+ led_r = LedOn;
+ if (led_r == LedOn) {
+ num_claw_turned_on_0++;
+ }
+
+ led_b = not LedOn;
+ led_g = not LedOn;
+
+ servo.position(27);
+ pc.printf("Servo position is: left \r\n");
+ break;
+ }
+ case 1: {
+ led_b = LedOn;
+ if (led_b == LedOn) {
+ num_claw_turned_on_1++;
+ }
+
+ led_r = not LedOn;
+ led_g = not LedOn;
+
+ servo.position(3);
+ pc.printf("Servo position is: center \r\n");
+ break;
+ }
+ case 2: {
+ led_g = LedOn;
+ if (led_g == LedOn) {
+ num_claw_turned_on_2++;
+ }
+
+ led_r = not LedOn;
+ led_b = not LedOn;
+
+ servo.position(-18);
+ pc.printf("Servo position is: right \r\n");
+ break;
+ }
+ }
+
+ }else{}
}
-
- // LEDs to turn off:
led_r = not LedOn;
led_g = not LedOn;
- A.speed(0) == 0;
+
break;
}
}
}
-/**
- * Print the number of times each LED was turned on through serial communication
- */
-void PrintSerial()
-{
- pc.printf("*Status*\r\n\tn_r = %d\r\n\tn_g = %d\r\n\tn_b = %d\r\n\r\n",
- num_turned_on_0, num_turned_on_1, num_turned_on_2);
-}
-/**
- * Switch the led id that blinks
- * led_blink_id goes from 0 -> 1 -> 2 -> 0 -> ...
- * @ensure led_blink_id = ++led_blink_id % kNumStates
- */
+// Switch the part
void SetValue2() {
part_id = 2;
}
@@ -120,36 +324,17 @@
}
-/**
- * Main loop.
- */
+// Main
int main()
{
- // Serial comm baud rate
- pc.baud(SERIAL_BAUD);
- pc.printf("\r\n**RESET**\r\n");
-
- // Turn off all LEDs initially
led_r = not LedOn;
led_g = not LedOn;
led_b = not LedOn;
- // Create ticker and attach LED toggle function
- Ticker tick_toggle_led;
- tick_toggle_led.attach(&SwitchPart,kTimeLedToggle);
-
- // Create ticker and attach Print function
- Ticker tick_print_serial;
- tick_print_serial.attach(&PrintSerial,kTimePrintSerial);
-
- // Create interrupt and attach switch function
- InterruptIn btn_cart(D2);
+ tick_part.attach(&SwitchPart,kTimeToggle);
+
btn_cart.fall(&SetValue2);
-
- InterruptIn btn_arm(D3);
btn_arm.fall(&SetValue3);
-
- InterruptIn btn_claw(D4);
btn_claw.fall(&SetValue4);
while (true);