Nick van den Berg / Mbed 2 deprecated The_Claw_with_EMG_Control_PID

Cleaner version

Dependencies:   HIDScope MODSERIAL Motordriver QEI Servo mbed

Fork of The_Claw_with_EMG_Control_PID by Meike Froklage

main.cpp@4:e62a2df0a5b5, 2016-10-19 (annotated)

Committer:
meikefrok
Date:
Wed Oct 19 14:03:19 2016 +0000
Revision:
4:e62a2df0a5b5
Parent:
3:3a671d01bcb8
Child:
5:3d88f7506cd9
Errors eruit gehaald

Who changed what in which revision?

UserRevisionLine numberNew contents of line
megrootens 0:048fbd80203e 1 #include "mbed.h"
meikefrok 3:3a671d01bcb8 2 #include "motordriver.h"
megrootens 0:048fbd80203e 3
megrootens 0:048fbd80203e 4 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
megrootens 0:048fbd80203e 5 // Serial communication using MODSERIAL
megrootens 0:048fbd80203e 6 #define SERIAL_BAUD 115200 // baud rate for serial communication
megrootens 0:048fbd80203e 7 #include "MODSERIAL.h"
megrootens 0:048fbd80203e 8 MODSERIAL pc(USBTX,USBRX);
megrootens 0:048fbd80203e 9
megrootens 0:048fbd80203e 10 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
megrootens 0:048fbd80203e 11 // Timing
megrootens 0:048fbd80203e 12 const float kTimeLedToggle = 0.25f; // period with which to toggle LED
megrootens 0:048fbd80203e 13 const float kTimePrintSerial = 1.0f;// period with which data is printed
megrootens 0:048fbd80203e 14
megrootens 0:048fbd80203e 15 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = >
megrootens 0:048fbd80203e 16 // constants
meikefrok 3:3a671d01bcb8 17 const int LedOn = 0; // LED on if 0
megrootens 0:048fbd80203e 18
megrootens 0:048fbd80203e 19 // LEDs
meikefrok 2:ad4b181a6422 20 DigitalOut led_r(LED_RED);
meikefrok 2:ad4b181a6422 21 DigitalOut led_g(LED_GREEN);
meikefrok 2:ad4b181a6422 22 DigitalOut led_b(LED_BLUE);
megrootens 0:048fbd80203e 23
megrootens 0:048fbd80203e 24 // ID of led that should blink; *volatile* because changed by interrupt
meikefrok 2:ad4b181a6422 25 volatile int part_id = 0;
megrootens 0:048fbd80203e 26
megrootens 0:048fbd80203e 27 // counters
megrootens 0:048fbd80203e 28 int num_turned_on_0 = 0; // count number of times red LED turned on
megrootens 0:048fbd80203e 29 int num_turned_on_1 = 0; // count number of times green LED turned on
megrootens 0:048fbd80203e 30 int num_turned_on_2 = 0; // count number of times blue LED turned on
megrootens 0:048fbd80203e 31
megrootens 0:048fbd80203e 32
meikefrok 3:3a671d01bcb8 33 //Safety for Motor
meikefrok 3:3a671d01bcb8 34 int Brakeable; //cna the motor driver break
meikefrok 3:3a671d01bcb8 35 int sign; //prevents throwing the motor from full foward to full reverse and stuff melting.
megrootens 0:048fbd80203e 36
meikefrok 3:3a671d01bcb8 37 //Motor
meikefrok 4:e62a2df0a5b5 38 Motor A(D6, D7, D7, Brakeable); // pwm, fwd, rev, brake (right)
meikefrok 4:e62a2df0a5b5 39 Motor B(D5, D4, D4, Brakeable); // pwm, fwd, rev, brake (left)
meikefrok 3:3a671d01bcb8 40
meikefrok 3:3a671d01bcb8 41
meikefrok 3:3a671d01bcb8 42
meikefrok 3:3a671d01bcb8 43
meikefrok 3:3a671d01bcb8 44 void SwitchPart()
megrootens 0:048fbd80203e 45 {
meikefrok 2:ad4b181a6422 46 switch (part_id) {
meikefrok 3:3a671d01bcb8 47 case 2: {
megrootens 0:048fbd80203e 48 // LED 0 to toggle and count:
meikefrok 3:3a671d01bcb8 49 led_r = LedOn;
meikefrok 3:3a671d01bcb8 50 if (led_r == LedOn) {
megrootens 0:048fbd80203e 51 num_turned_on_0++;
megrootens 0:048fbd80203e 52 }
megrootens 0:048fbd80203e 53
megrootens 0:048fbd80203e 54 // LEDs to turn off:
meikefrok 3:3a671d01bcb8 55 led_g = not LedOn;
meikefrok 3:3a671d01bcb8 56 led_b = not LedOn;
meikefrok 3:3a671d01bcb8 57 A.speed(0) == 0;
meikefrok 3:3a671d01bcb8 58 B.speed(0) == 0;
megrootens 0:048fbd80203e 59
megrootens 0:048fbd80203e 60 break;
megrootens 0:048fbd80203e 61 }
meikefrok 3:3a671d01bcb8 62 case 3: {
megrootens 0:048fbd80203e 63 // LED 1 to toggle and count:
meikefrok 3:3a671d01bcb8 64 led_g = LedOn;
meikefrok 3:3a671d01bcb8 65 A.speed(0.5) == 0.5;
meikefrok 3:3a671d01bcb8 66 if (led_g == LedOn) {
megrootens 0:048fbd80203e 67 num_turned_on_1++;
megrootens 0:048fbd80203e 68 }
megrootens 0:048fbd80203e 69
megrootens 0:048fbd80203e 70 // LEDs to turn off:
meikefrok 3:3a671d01bcb8 71 led_r = not LedOn;
meikefrok 3:3a671d01bcb8 72 led_b = not LedOn;
meikefrok 3:3a671d01bcb8 73 B.speed(0) == 0;
meikefrok 3:3a671d01bcb8 74
megrootens 0:048fbd80203e 75 break;
megrootens 0:048fbd80203e 76 }
meikefrok 3:3a671d01bcb8 77 case 4: {
megrootens 0:048fbd80203e 78 // LED 2 to toggle and count:
meikefrok 3:3a671d01bcb8 79 led_b = LedOn;
meikefrok 3:3a671d01bcb8 80 B.speed(0.5) == 0.5;
meikefrok 3:3a671d01bcb8 81 if (led_b == LedOn) {
megrootens 0:048fbd80203e 82 num_turned_on_2++;
megrootens 0:048fbd80203e 83 }
megrootens 0:048fbd80203e 84
megrootens 0:048fbd80203e 85 // LEDs to turn off:
meikefrok 3:3a671d01bcb8 86 led_r = not LedOn;
meikefrok 3:3a671d01bcb8 87 led_g = not LedOn;
meikefrok 3:3a671d01bcb8 88 A.speed(0) == 0;
megrootens 0:048fbd80203e 89
megrootens 0:048fbd80203e 90 break;
megrootens 0:048fbd80203e 91 }
megrootens 0:048fbd80203e 92 }
megrootens 0:048fbd80203e 93 }
megrootens 0:048fbd80203e 94
megrootens 0:048fbd80203e 95 /**
megrootens 0:048fbd80203e 96 * Print the number of times each LED was turned on through serial communication
megrootens 0:048fbd80203e 97 */
megrootens 0:048fbd80203e 98 void PrintSerial()
megrootens 0:048fbd80203e 99 {
megrootens 0:048fbd80203e 100 pc.printf("*Status*\r\n\tn_r = %d\r\n\tn_g = %d\r\n\tn_b = %d\r\n\r\n",
megrootens 0:048fbd80203e 101 num_turned_on_0, num_turned_on_1, num_turned_on_2);
megrootens 0:048fbd80203e 102 }
megrootens 0:048fbd80203e 103
megrootens 0:048fbd80203e 104 /**
megrootens 0:048fbd80203e 105 * Switch the led id that blinks
megrootens 0:048fbd80203e 106 * led_blink_id goes from 0 -> 1 -> 2 -> 0 -> ...
megrootens 0:048fbd80203e 107 * @ensure led_blink_id = ++led_blink_id % kNumStates
megrootens 0:048fbd80203e 108 */
meikefrok 2:ad4b181a6422 109 void SetValue2() {
meikefrok 3:3a671d01bcb8 110 part_id = 2;
meikefrok 2:ad4b181a6422 111 }
meikefrok 2:ad4b181a6422 112
meikefrok 2:ad4b181a6422 113 void SetValue3() {
meikefrok 3:3a671d01bcb8 114 part_id = 3;
meikefrok 2:ad4b181a6422 115 }
meikefrok 2:ad4b181a6422 116
meikefrok 2:ad4b181a6422 117
meikefrok 2:ad4b181a6422 118 void SetValue4() {
meikefrok 3:3a671d01bcb8 119 part_id = 4;
megrootens 0:048fbd80203e 120 }
megrootens 0:048fbd80203e 121
meikefrok 2:ad4b181a6422 122
megrootens 0:048fbd80203e 123 /**
megrootens 0:048fbd80203e 124 * Main loop.
megrootens 0:048fbd80203e 125 */
megrootens 0:048fbd80203e 126 int main()
megrootens 0:048fbd80203e 127 {
megrootens 0:048fbd80203e 128 // Serial comm baud rate
megrootens 0:048fbd80203e 129 pc.baud(SERIAL_BAUD);
megrootens 0:048fbd80203e 130 pc.printf("\r\n**RESET**\r\n");
megrootens 0:048fbd80203e 131
megrootens 0:048fbd80203e 132 // Turn off all LEDs initially
meikefrok 3:3a671d01bcb8 133 led_r = not LedOn;
meikefrok 3:3a671d01bcb8 134 led_g = not LedOn;
meikefrok 3:3a671d01bcb8 135 led_b = not LedOn;
megrootens 0:048fbd80203e 136
megrootens 0:048fbd80203e 137 // Create ticker and attach LED toggle function
megrootens 0:048fbd80203e 138 Ticker tick_toggle_led;
meikefrok 3:3a671d01bcb8 139 tick_toggle_led.attach(&SwitchPart,kTimeLedToggle);
megrootens 0:048fbd80203e 140
megrootens 0:048fbd80203e 141 // Create ticker and attach Print function
megrootens 0:048fbd80203e 142 Ticker tick_print_serial;
megrootens 0:048fbd80203e 143 tick_print_serial.attach(&PrintSerial,kTimePrintSerial);
megrootens 0:048fbd80203e 144
megrootens 0:048fbd80203e 145 // Create interrupt and attach switch function
meikefrok 3:3a671d01bcb8 146 InterruptIn btn_cart(D2);
meikefrok 3:3a671d01bcb8 147 btn_cart.fall(&SetValue2);
megrootens 0:048fbd80203e 148
meikefrok 3:3a671d01bcb8 149 InterruptIn btn_arm(D3);
meikefrok 3:3a671d01bcb8 150 btn_arm.fall(&SetValue3);
meikefrok 2:ad4b181a6422 151
meikefrok 4:e62a2df0a5b5 152 InterruptIn btn_claw(D4);
meikefrok 3:3a671d01bcb8 153 btn_claw.fall(&SetValue4);
meikefrok 2:ad4b181a6422 154
megrootens 0:048fbd80203e 155 while (true);
megrootens 0:048fbd80203e 156 }
meikefrok 2:ad4b181a6422 157
meikefrok 2:ad4b181a6422 158
meikefrok 2:ad4b181a6422 159