Commit 83d5d3ef authored by Alex Wang Fu's avatar Alex Wang Fu
Browse files

added verbose flag + unsuccsesfully tried to make soft reboot compliant

parent f0c1d392
#include "ports/atmel-samd/common-hal/motor_control/PID.h"
#include "ports/atmel-samd/common-hal/motor_control/Control.h"
#include "peripherals/samd/pins.h"
#include "py/runtime.h"
#include <stdio.h>
#include <stdbool.h>
......@@ -19,6 +20,7 @@
#define RESET_NUM_WING (float) 1000.0 // this error should pause the wings + reset motors
#define EPSILON 0.005 // The threshold in which two floats are conisdered equal
static bool verbose = true;
extern void set_pwm(bool left, bool up, float duty);
......@@ -57,7 +59,7 @@ float get_wing_angle(bool left){ ///Will take in sensor values from WING acceler
bird.z < 9.0 || bird.z > 11.0) {
// Return non-possible number because bird acceleration does not make sense (garbage value)
printf("CENTER RAW: %f %f %f \t", (double)bird.x, (double)bird.y, (double)bird.z);
printf("Center accel halting. CENTER RAW: %f %f %f \t", (double)bird.x, (double)bird.y, (double)bird.z);
return ERROR_HALT;
}
......@@ -68,10 +70,14 @@ float get_wing_angle(bool left){ ///Will take in sensor values from WING acceler
//accelerometer value is read (for the test done in the Python script, the pinout for the body accelerometer was used)
if (left) { //for left wing
accel = get_raw_accel(LEFTWING);
printf("LEFT RAW: %f %f %f \t", (double)accel.x, (double)accel.y, (double)accel.z);
if(verbose) {
printf("LEFT RAW: %f %f %f \t", (double)accel.x, (double)accel.y, (double)accel.z);
}
} else { //for right wing
accel = get_raw_accel(RIGHTWING);
printf("RIGHT RAW: %f %f %f \t", (double)accel.x, (double)accel.y, (double)accel.z);
if(verbose) {
printf("RIGHT RAW: %f %f %f \t", (double)accel.x, (double)accel.y, (double)accel.z);
}
}
accel_x = accel.x; //X acceleration value from wing sensor
accel_y = accel.y; //Y acceleration value from wing sensor
......@@ -139,12 +145,8 @@ void set_wing_angle(bool left, int degrees_C){
if(left) {
printf("Current_angle: %f, left wing error; halting\t", (double)current_angle);
// acceleration accel = get_raw_accel(LEFTWING);
// printf("LEFT RAW: %f %f %f \t", (double)accel.x, (double)accel.y, (double)accel.z);
}else {
printf("Current_angle: %f, right wing error; halting\n", (double)current_angle);
// acceleration accel = get_raw_accel(RIGHTWING);
// printf("RIGHT RAW: %f %f %f \t", (double)accel.x, (double)accel.y, (double)accel.z);
}
return;
......@@ -207,22 +209,31 @@ void set_wing_angle(bool left, int degrees_C){
if (current_angle < degrees_C){
if(left){ //send the PWM signal to move the left wing up
if(verbose) {
printf("LEFT UP: Goal: %d, current: %f \t", degrees_C, (double)current_angle);
set_pwm(LEFT,UP,duty);
}
set_pwm(LEFT,UP,duty);
}
else{ //send the PWM signal moves the right wing up
if(verbose) {
printf("RIGHT UP: Goal: %d, current: %f \n", degrees_C, (double)current_angle);
set_pwm(RIGHT,UP,duty);
}
set_pwm(RIGHT,UP,duty);
}
} else {
if(left){ //send a PWM signal to stop the left wing (TODO: program this so that it moves the left wing down)
if(verbose) {
printf("LEFT DOWN: Goal: %d, current: %f\t", degrees_C, (double)current_angle);
set_pwm(LEFT, DOWN, duty);
}
set_pwm(LEFT, DOWN, duty);
}
else{ //send a PWM signal to stop the right wing (TODO: program this so that it moves the right wing down)
if(verbose) {
printf("RIGHT DOWN: Goal: %d, current: %f\n", degrees_C, (double)current_angle);
set_pwm(RIGHT, DOWN, duty);
}
set_pwm(RIGHT, DOWN, duty);
}
}
}
......
......@@ -171,17 +171,25 @@ static bool motor_control_initted = false;
void shared_module_ee285_motor_control_loop_init(void) {
if(motor_control_initted) {
return;
printf("motor control loop already initted!\n");
}
// Init accels
init_accels();
// Init accels only if motor control not already initted
if(!motor_control_initted){
printf("initting accels\n");
init_accels();
// Init interrupts
printf("initting interrupts\n");
common_hal_motor_construct();
}
// Stretch goal: refactor PWM so that it has an explicit init, and then init
// PWM here
// Init interrupts
common_hal_motor_construct();
// Start interrupts any time motor control loop is initted, regardless of whether it's the first time
// because it seems like on soft reboots, while static variables remain set, interrupts
// need to be re started.
printf("starting interrupts\n");
common_hal_motor_PID();
motor_control_initted = true;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment