git.haldean.org droidcopter / fe378cd
Update firmware for better stability at low speeds. Will Brown 11 years ago
2 changed file(s) with 38 addition(s) and 39 deletion(s). Raw diff Collapse all Expand all
77 #define STATUS_LED 13
88 #define STATUS_LED_CYCLES 30000
99 #define WORD_BUFFER_LENGTH 4
10 #define WORD_LENGTH 3
10 #define WORD_LENGTH 2
1111
12 #include <SoftwareServo.h>
12 #include <Servo.h>
1313
1414 struct motor {
1515 unsigned char pin;
1616 unsigned int current_speed;
1717 unsigned int next_speed;
18 SoftwareServo ctrl;
18 Servo ctrl;
1919 };
2020
2121 struct motor motors[MOTOR_COUNT] = {
22 {6, 0, 0, SoftwareServo()}, {9, 0, 0, SoftwareServo()},
23 {10, 0, 0, SoftwareServo()}, {11, 0, 0, SoftwareServo()}
22 {6, 0, 0, Servo()}, {9, 0, 0, Servo()},
23 {10, 0, 0, Servo()}, {11, 0, 0, Servo()}
2424 };
2525
2626 char buffer[WORD_BUFFER_LENGTH];
6868 * Initialize messenger service and speed vector.
6969 */
7070 void setup(void) {
71 /* Initialize serial monitor. */
72 Serial.begin(115200);
73
7174 /* Set the serial status LED to output. */
7275 pinMode(STATUS_LED, OUTPUT);
7376
7477 /* Initialize the motor controllers. */
7578 init_motors();
7679
77 /* Initialize serial monitor. */
78 Serial.begin(115200);
80 Serial.println("BEGIN");
7981 }
8082
8183 /**
98100 }
99101
100102 buffer[WORD_LENGTH] = '\0';
101 motors[i].next_speed = atoi(buffer);
103 motors[i].next_speed = map(atoi(buffer), -1, 99, 67, 160);
102104 } else {
103105 Serial.println(BAD_REQUEST);
104106 }
119121 digitalWrite(STATUS_LED, LOW);
120122 }
121123
124 #ifdef ENABLE_HEARTBEAT
122125 if (led_cycles % 10000 == 0) {
123 // Serial.println(HEARTBEAT_PULSE);
126 Serial.println(HEARTBEAT_PULSE);
124127 }
128 #endif
125129
126130 }
127131
0 #include <SoftwareServo.h>
0 #include <Servo.h>
11
22 // Motors
33 #define PIN_FRONT 11
4 #define PIN_REAR 5
5 #define PIN_LEFT 6
6 #define PIN_RIGHT 7
4 #define PIN_REAR 9
5 #define PIN_LEFT 10
6 #define PIN_RIGHT 6
77 #define MINCOMMAND 0
88
9 struct Motor {
9 struct Motors {
1010 int command[4];
1111 int axisCommand[3];
1212 boolean armed;
1818 1000
1919 };
2020
21 SoftwareServo motorFront;
22 SoftwareServo motorRear;
23 SoftwareServo motorLeft;
24 SoftwareServo motorRight;
21 Servo motorFront;
22 Servo motorRear;
23 Servo motorLeft;
24 Servo motorRight;
2525
2626 void setupMotors() {
2727 motorFront.attach(PIN_FRONT);
3232 // Arm ESCs
3333 setAllMotors(MINCOMMAND);
3434 updateMotors();
35 unsigned long tempTime = millis();
36 while(millis()<tempTime+1000*4) {
37 SoftwareServo::refresh();
35 }
36
37 void setup() {
38 setupMotors();
39 }
40
41 void loop() {;}
42
43 void setAllMotors(int cmd) {
44 int i;
45 for (i=0; i<4; i++) {
46 motor.command[i] = cmd;
3847 }
3948 }
4049
41 void wait(int ms) {
42 int temp = millis();
43 while(millis()-temp<ms) {
44 SoftwareServo::refresh();
50 void updateMotors() {
51 int i;
52 for (i=0; i<4; i++) {
53 motorFront.write(motor.command[i]);
4554 }
4655 }
4756
48 void setAllMotors(int cmd) {
49 motor.command[MOTOR_FRONT] = cmd;
50 motor.command[MOTOR_REAR] = cmd;
51 motor.command[MOTOR_LEFT] = cmd;
52 motor.command[MOTOR_RIGHT] = cmd;
53 }
54
55 void updateMotors() {
56 motorFront.write(motor.command[MOTOR_FRONT]);
57 motorRear.write(motor.command[MOTOR_REAR]);
58 motorLeft.write(motor.command[MOTOR_LEFT]);
59 motorRight.write(motor.command[MOTOR_RIGHT]);
60 }
61