Embedded Team / Mbed 2 deprecated Tracking Mbed Servo

Tracking Mbed Servo

Dependencies:   C12832 Servo mbed-rtos mbed

main.cpp@0:89523986a177, 2015-02-26 (annotated)

Committer:
Qoramas
Date:
Thu Feb 26 16:48:02 2015 +0000
Revision:
0:89523986a177
Child:
1:a22303e03da3
first commit

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
Qoramas 0:89523986a177 17 static float clamp(float value, float min, float max){
Qoramas 0:89523986a177 18 return (value < min)? min : (value > max)? max : value;
Qoramas 0:89523986a177 19 }
Qoramas 0:89523986a177 20
Qoramas 0:89523986a177 21 void getUserPosition(void const *name){
Qoramas 0:89523986a177 22 while(1){
Qoramas 0:89523986a177 23 pc.scanf("%f,%f", &userX, &userY);
Qoramas 0:89523986a177 24 }
Qoramas 0:89523986a177 25 }
Qoramas 0:89523986a177 26
Qoramas 0:89523986a177 27 void getSensorValue(void const *name){
Qoramas 0:89523986a177 28 while(1){
Qoramas 0:89523986a177 29 sensorIn = aIn.read();
Qoramas 0:89523986a177 30 Thread::wait(50);
Qoramas 0:89523986a177 31 }
Qoramas 0:89523986a177 32 }
Qoramas 0:89523986a177 33
Qoramas 0:89523986a177 34 void logic(void const *name){
Qoramas 0:89523986a177 35 while(1){
Qoramas 0:89523986a177 36
Qoramas 0:89523986a177 37 servoOutX = clamp(userX, 0, 1);
Qoramas 0:89523986a177 38 servoOutY = clamp(userY, 0, 1);
Qoramas 0:89523986a177 39
Qoramas 0:89523986a177 40 //Displays debug data
Qoramas 0:89523986a177 41 lcd.cls();
Qoramas 0:89523986a177 42 lcd.locate(0,0);
Qoramas 0:89523986a177 43 lcd.printf("InX: %f, OutX: %f", userX, servoOutX);
Qoramas 0:89523986a177 44 lcd.locate(0,15);
Qoramas 0:89523986a177 45 lcd.printf("InY: %f, OutY: %f", userY, servoOutY);
Qoramas 0:89523986a177 46
Qoramas 0:89523986a177 47 Thread::wait(50);
Qoramas 0:89523986a177 48
Qoramas 0:89523986a177 49 }
Qoramas 0:89523986a177 50 }
Qoramas 0:89523986a177 51
Qoramas 0:89523986a177 52 //Sets the servos to the given data
Qoramas 0:89523986a177 53 void setServoValue(void const *name){
Qoramas 0:89523986a177 54 while(1){
Qoramas 0:89523986a177 55
Qoramas 0:89523986a177 56 //Set the servos to the set values
Qoramas 0:89523986a177 57 servoPan = servoOutX;
Qoramas 0:89523986a177 58 servoTilt = servoOutY;
Qoramas 0:89523986a177 59
Qoramas 0:89523986a177 60 Thread::wait(50);
Qoramas 0:89523986a177 61 }
Qoramas 0:89523986a177 62 }
Qoramas 0:89523986a177 63
Qoramas 0:89523986a177 64 int main(){
Qoramas 0:89523986a177 65 //Start each of the threads
Qoramas 0:89523986a177 66 Thread t1(getUserPosition, (void *)"Th 1");
Qoramas 0:89523986a177 67 Thread t2(getSensorValue, (void *)"Th 2");
Qoramas 0:89523986a177 68 Thread t3(logic, (void *)"Th 3");
Qoramas 0:89523986a177 69 Thread t4(setServoValue, (void *) "Th 4");
Qoramas 0:89523986a177 70
Qoramas 0:89523986a177 71 //Stop the main process from dying
Qoramas 0:89523986a177 72 while(1)wait(10000);
Qoramas 0:89523986a177 73 }