mirror of
https://github.com/2martens/uni.git
synced 2026-05-07 11:56:26 +02:00
139 lines
2.8 KiB
C++
139 lines
2.8 KiB
C++
#include <Servo.h>
|
|
|
|
// these variables describe the used hardware pins
|
|
// adjust them when you use other pins
|
|
// hardware pins
|
|
int az = 50;
|
|
|
|
int xout = A4;
|
|
int zout = A2;
|
|
int vref = A3;
|
|
int ledPin = 13;
|
|
|
|
// servo
|
|
Servo ourServo;
|
|
int servoPin = 11;
|
|
|
|
// used to achieve a 10 Hz frequency
|
|
// don't touch them
|
|
long rc = 1049999;
|
|
|
|
// flags for servo
|
|
bool volatile cwMaxReached = false;
|
|
bool volatile ccwMaxReached = false;
|
|
bool volatile lightLED = false;
|
|
|
|
bool volatile read_ready = false;
|
|
|
|
|
|
/**
|
|
* Validates the servo position.
|
|
*
|
|
* @param int newServoPos
|
|
*/
|
|
int validate_new_servo_pos(int newServoPos) {
|
|
if (newServoPos > 159) {
|
|
newServoPos = 159;
|
|
cwMaxReached = true;
|
|
lightLED = true;
|
|
Serial.println("Interval 25-159");
|
|
}
|
|
|
|
if (newServoPos < 25) {
|
|
newServoPos = 25;
|
|
ccwMaxReached = true;
|
|
lightLED = true;
|
|
Serial.println("Interval 25-159");
|
|
}
|
|
|
|
return newServoPos;
|
|
}
|
|
|
|
int parse_angle(String command) {
|
|
int indexP1 = command.indexOf('(');
|
|
int indexP2 = command.indexOf(')');
|
|
String command2 = command;
|
|
command2.remove(indexP1);
|
|
if (command2 != "moveTo") {
|
|
Serial.println("Use moveTo(<angle>)");
|
|
return ourServo.read();
|
|
}
|
|
|
|
command.remove(indexP2);
|
|
int angle = command.substring(indexP1 + 1).toInt();
|
|
|
|
return angle;
|
|
}
|
|
|
|
|
|
/**
|
|
* Setup function for initial setup code
|
|
*/
|
|
void setup() {
|
|
pmc_set_writeprotect(false);
|
|
pmc_enable_periph_clk(ID_TC1);
|
|
|
|
// configure hardware timer
|
|
TC_Configure(TC0, 1, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | TC_CMR_TCCLKS_TIMER_CLOCK2) ;
|
|
TC_SetRC(TC0, 1, rc);
|
|
|
|
TC0->TC_CHANNEL[1].TC_IER=TC_IER_CPCS; // IER = interrupt enable register
|
|
TC0->TC_CHANNEL[1].TC_IDR=~TC_IER_CPCS;
|
|
|
|
NVIC_ClearPendingIRQ(TC1_IRQn);
|
|
NVIC_EnableIRQ(TC1_IRQn);
|
|
|
|
// start hardware timer
|
|
//TC_Start(TC0, 1);
|
|
|
|
// Configure pins
|
|
pinMode(ledPin, OUTPUT);
|
|
pinMode(az, OUTPUT);
|
|
digitalWrite(az, HIGH);
|
|
digitalWrite(ledPin, LOW);
|
|
|
|
ourServo.attach(servoPin);
|
|
ourServo.write(90);
|
|
|
|
// initialize serial port
|
|
Serial.begin(9600);
|
|
}
|
|
|
|
/**
|
|
* Loop function for main code
|
|
*/
|
|
void loop() {
|
|
if (Serial.available() > 0) {
|
|
String command;
|
|
char currentChar;
|
|
int i = 0;
|
|
bool readable = true;
|
|
command = Serial.readString();
|
|
|
|
int servoPos = parse_angle(command);
|
|
int validatedServoPos = validate_new_servo_pos(servoPos);
|
|
|
|
Serial.print("move to angle ");
|
|
Serial.println(validatedServoPos);
|
|
|
|
ourServo.write(validatedServoPos);
|
|
|
|
if (lightLED) {
|
|
lightLED = false;
|
|
digitalWrite(ledPin, HIGH);
|
|
delay(500);
|
|
digitalWrite(ledPin, LOW);
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Used to handle the timer.
|
|
*/
|
|
void TC1_Handler()
|
|
{
|
|
// request static for some magic behind the curtain
|
|
TC_GetStatus(TC0, 1);
|
|
read_ready = true;
|
|
}
|