winch servo - cchamchi/cansat GitHub Wiki

#include <CansatDebug.h>
#include <CansatSystem.h>
#include <Servo.h>


Servo winch;
void setup() {
  // put your setup code here, to run once:
  // cansat initialization
  CansatSystemInit();  
  // Init Servo
  winch.attach(D0);
  winch.writeMicroseconds(1500);  // neutral position
  delay(20);

  // Init BT
  Serial.begin(9600);
  Serial.write("+++\r\n");
  Serial.print("at+btmode,3\r\n");
  Serial.print("at+uartconfig,115200,N,1,0\r\n");
  Serial.print("atz\r\n");
  Serial.begin(115200);
}

void loop() {
  // put your main code here, to run repeatedly:
  if(Serial.available()){
    long angle = Serial.parseInt();
    if(angle!=0){
      winch.writeMicroseconds(angle);  //900~2100 microsecond
      Serial.println(angle);
    }    
  }
}