Embedded Team / Mbed 2 deprecated Tracking Mbed Servo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   C12832 Servo mbed-rtos mbed

main.cpp@1:a22303e03da3, 2015-03-02 (annotated)

Committer:
Argensis
Date:
Mon Mar 02 14:46:00 2015 +0000
Revision:
1:a22303e03da3
Parent:
0:89523986a177
Child:
2:b1e22f332443
Use actual IPC instead of a shared variable

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Qoramas 0:89523986a177 1 #include "mbed.h"
Qoramas 0:89523986a177 2 #include "C12832.h"
Qoramas 0:89523986a177 3 #include "Servo.h"
Qoramas 0:89523986a177 4 #include "rtos.h"
Qoramas 0:89523986a177 5
Qoramas 0:89523986a177 6 C12832 lcd(p5, p7, p6, p8, p11);
Qoramas 0:89523986a177 7 Serial pc(USBTX, USBRX);
Qoramas 0:89523986a177 8 AnalogIn aIn(p17);
Qoramas 0:89523986a177 9
Qoramas 0:89523986a177 10 float userX, userY;
Qoramas 0:89523986a177 11 float servoOutX, servoOutY;
Qoramas 0:89523986a177 12 float sensorIn;
Qoramas 0:89523986a177 13
Qoramas 0:89523986a177 14 Servo servoPan(p21);
Qoramas 0:89523986a177 15 Servo servoTilt(p22);
Qoramas 0:89523986a177 16
Argensis 1:a22303e03da3 17 typedef struct {
Argensis 1:a22303e03da3 18 float x;
Argensis 1:a22303e03da3 19 float y;
Argensis 1:a22303e03da3 20 } Position;
Argensis 1:a22303e03da3 21
Argensis 1:a22303e03da3 22 Mail<Position, 16> rrInput;
Argensis 1:a22303e03da3 23
Qoramas 0:89523986a177 24 static float clamp(float value, float min, float max){
Qoramas 0:89523986a177 25 return (value < min)? min : (value > max)? max : value;
Qoramas 0:89523986a177 26 }
Qoramas 0:89523986a177 27
Argensis 1:a22303e03da3 28 bool withinDelta(float a, float b, float delta) {
Argensis 1:a22303e03da3 29 return fabs(a - b) < delta;
Argensis 1:a22303e03da3 30 }
Argensis 1:a22303e03da3 31
Qoramas 0:89523986a177 32 void getUserPosition(void const *name){
Argensis 1:a22303e03da3 33 float x;
Argensis 1:a22303e03da3 34 float y;
Argensis 1:a22303e03da3 35 float lastX;
Argensis 1:a22303e03da3 36 float lastY;
Argensis 1:a22303e03da3 37 pc.scanf("%f,%f", &x, &y);
Argensis 1:a22303e03da3 38 lastX = x;
Argensis 1:a22303e03da3 39 lastY = y;
Argensis 1:a22303e03da3 40 bool firstTime = true;
Qoramas 0:89523986a177 41 while(1){
Argensis 1:a22303e03da3 42 pc.scanf("%f,%f", &x, &y);
Argensis 1:a22303e03da3 43 if(firstTime || !withinDelta(x, lastX, 0.02) || !withinDelta(y, lastY, 0.02)) {
Argensis 1:a22303e03da3 44 firstTime = false;
Argensis 1:a22303e03da3 45 Position* pos = rrInput.alloc();
Argensis 1:a22303e03da3 46 if(pos != NULL) { // Can't have nmore than 16 items in the queue, alloc() returns null if queue is full.
Argensis 1:a22303e03da3 47 pos->x = x;
Argensis 1:a22303e03da3 48 pos->y = y;
Argensis 1:a22303e03da3 49 rrInput.put(pos);
Argensis 1:a22303e03da3 50 }
Argensis 1:a22303e03da3 51 lastX = x;
Argensis 1:a22303e03da3 52 lastY = y;
Argensis 1:a22303e03da3 53 }
Argensis 1:a22303e03da3 54
Qoramas 0:89523986a177 55 }
Qoramas 0:89523986a177 56 }
Qoramas 0:89523986a177 57
Qoramas 0:89523986a177 58 void getSensorValue(void const *name){
Qoramas 0:89523986a177 59 while(1){
Qoramas 0:89523986a177 60 sensorIn = aIn.read();
Qoramas 0:89523986a177 61 Thread::wait(50);
Qoramas 0:89523986a177 62 }
Qoramas 0:89523986a177 63 }
Qoramas 0:89523986a177 64
Qoramas 0:89523986a177 65 void logic(void const *name){
Qoramas 0:89523986a177 66 while(1){
Argensis 1:a22303e03da3 67 osEvent evt = rrInput.get();
Argensis 1:a22303e03da3 68 if(evt.status == osEventMail) {
Argensis 1:a22303e03da3 69 Position* reading = (Position*)evt.value.p;
Argensis 1:a22303e03da3 70 lcd.cls();
Argensis 1:a22303e03da3 71 lcd.locate(0, 0);
Argensis 1:a22303e03da3 72 servoOutX = clamp(reading->x, 0, 1);
Argensis 1:a22303e03da3 73 servoOutY = clamp(reading->y, 0, 1);
Argensis 1:a22303e03da3 74
Argensis 1:a22303e03da3 75 //Displays debug data
Argensis 1:a22303e03da3 76 lcd.cls();
Argensis 1:a22303e03da3 77 lcd.locate(0,0);
Argensis 1:a22303e03da3 78 lcd.printf("InX: %.3f, OutX: %.3f", reading->x, servoOutX);
Argensis 1:a22303e03da3 79 lcd.locate(0,15);
Argensis 1:a22303e03da3 80 lcd.printf("InY: %.3f, OutY: %.3f", reading->y, servoOutY);
Argensis 1:a22303e03da3 81
Argensis 1:a22303e03da3 82 rrInput.free(reading);
Argensis 1:a22303e03da3 83 Thread::wait(50);
Argensis 1:a22303e03da3 84 }
Qoramas 0:89523986a177 85 }
Qoramas 0:89523986a177 86 }
Qoramas 0:89523986a177 87
Qoramas 0:89523986a177 88 //Sets the servos to the given data
Qoramas 0:89523986a177 89 void setServoValue(void const *name){
Qoramas 0:89523986a177 90 while(1){
Qoramas 0:89523986a177 91
Qoramas 0:89523986a177 92 //Set the servos to the set values
Qoramas 0:89523986a177 93 servoPan = servoOutX;
Qoramas 0:89523986a177 94 servoTilt = servoOutY;
Qoramas 0:89523986a177 95
Qoramas 0:89523986a177 96 Thread::wait(50);
Qoramas 0:89523986a177 97 }
Qoramas 0:89523986a177 98 }
Qoramas 0:89523986a177 99
Qoramas 0:89523986a177 100 int main(){
Qoramas 0:89523986a177 101 //Start each of the threads
Qoramas 0:89523986a177 102 Thread t1(getUserPosition, (void *)"Th 1");
Qoramas 0:89523986a177 103 Thread t2(getSensorValue, (void *)"Th 2");
Qoramas 0:89523986a177 104 Thread t3(logic, (void *)"Th 3");
Qoramas 0:89523986a177 105 Thread t4(setServoValue, (void *) "Th 4");
Qoramas 0:89523986a177 106
Qoramas 0:89523986a177 107 //Stop the main process from dying
Qoramas 0:89523986a177 108 while(1)wait(10000);
Qoramas 0:89523986a177 109 }