-
Notifications
You must be signed in to change notification settings - Fork 0
/
RTOS_implementation_emgandservo.ino
98 lines (70 loc) · 1.99 KB
/
RTOS_implementation_emgandservo.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
#include <Arduino_FreeRTOS.h>
#include <Wire.h>
#include <semphr.h>
#include <Adafruit_PWMServoDriver.h>
#define MIN 75 //0°
#define MAX 560 // 180°
int pin = A0;
int sEMG_val = 0;
int testvar = 1000;
Adafruit_PWMServoDriver servoDriver = Adafruit_PWMServoDriver();
SemaphoreHandle_t xSerialSemaphore;
void Task1(void *pvParameters);
void Task2(void *pvParameters);
void setup(){
Serial.begin(9600);
servoDriver.begin();
servoDriver.setPWMFreq(50);
while(!Serial){
;
}
if(xSerialSemaphore == NULL){
xSerialSemaphore = xSemaphoreCreateMutex();
if((xSerialSemaphore) != NULL){
xSemaphoreGive((xSerialSemaphore));
}
}
xTaskCreate(Task1, "sEMGSensor", 256, NULL, 1, NULL);
xTaskCreate(Task2, "servoTask", 128, NULL, 1, NULL);
vTaskStartScheduler();
}
void loop(){
}
void Task1(void *pvParameters __attribute__((unused))){
(void) pvParameters;
const int duration = 500; //ms
const int amount = duration/50; //amount every 50ms
int sEMG_val_array[amount];
for(;;){
sEMG_val = analogRead(pin);
int sum = 0;
for(int i = 0; i < amount; i++){
sEMG_val_array[i] = analogRead(pin);
sum += sEMG_val_array[i]
vTaskDelay(pdMS_TO_TICKS(50));
}
int sEMG_val = sum / amount;
if(xSemaphoreTake(xSerialSemaphore, (TickType_t) 5) == pdTRUE){
Serial.println(sEMG_val);
xSemaphoreGive(xSerialSemaphore);
}
vTaskDelay(pdMS_TO_TICKS(50));
}
}
void Task2(void *pvParameters __attribute((unused))){
(void) pvParameters;
for(;;){
int sEMG_val_local = sEMG_val;
if(xSemaphoreTake(xSerialSemaphore, (TickTye_t) 5) == pdTRUE){
int angle = map(sEMG_val_local, 0, 1023, MIN, MAX);
servoDriver.setPWM(0, 0, angle);
servoDriver.setPWM(1, 0, angle);
servoDriver.setPWM(2, 0, angle);
servoDriver.setPWM(3, 0, angle);
servoDriver.setPWM(4, 0, angle);
vTaskDelay(pdMS_TO_TICKS(500));
xSemaphoreGive(xSerialSemaphore);
}
vTaskDelay(pdMS_TO_TICKS(50));
}
}