Ironcup Mar 2020
Dependencies: mbed mbed-rtos MotionSensor EthernetInterface
Diff: main.cpp
- Revision:
- 18:c1cd11db47ed
- Parent:
- 16:a9e0eb97557f
- Child:
- 20:7138ab2f93f7
--- a/main.cpp Mon May 02 00:58:20 2016 +0000 +++ b/main.cpp Sun May 15 19:13:53 2016 +0000 @@ -99,7 +99,7 @@ Ticker controller_ticker; // Magnetometer variables and functions -float max_x, max_y, min_x, min_y,x,y; +float max_x=0, max_y=0, min_x=0, min_y=0,x,y; MotionSensorDataUnits mag_data; MotionSensorDataCounts mag_raw; float processMagAngle(); @@ -118,18 +118,18 @@ motor.setVelocity(0); // Protocol parameters + printf("Initializing Ethernet!\r\n"); set_leds_color(RED, red_led, green_led, blue_led); eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR); eth.connect(); + printf("Protocol initialized! \r\n"); set_leds_color(BLUE, red_led, green_led, blue_led); rcv.set_socket(); - gyro.start_measure(GYRO_PERIOD); controller_ticker.attach(&control,Ts); //main loop while(1){ readProtocol(); - wait(0.01); } } void control(){ @@ -139,75 +139,64 @@ if(!rcv.receive()) return; char msg = rcv.get_msg(); + printf("Message received!"); switch(msg) { case NONE: - //ser.printf("sending red signal to led\r\n"); - red_led = LED_ON; - + //ser.printf("sending red signal to led\r\n"); + set_leds_color(BLACK,red_led,green_led,blue_led); printf("NONE\r\n"); - wait(1); - red_led = LED_OFF; break; case BRAKE: //ser.printf("sending green signal to led\r\n"); - green_led = LED_ON; + set_leds_color(YELLOW,red_led,green_led,blue_led); motor.stopJogging(); printf("BRAKE\r\n"); motor.brakeMotor(); - green_led = LED_OFF; break; case ANG_RST: //ser.printf("sending blue signal to led\r\n"); - blue_led = LED_ON; - + set_leds_color(PURPLE,red_led,green_led,blue_led); printf("ANG_RST\r\n"); gyro.stop_measure(); + printf("Stopped gyro\r\n"); gyro.start_measure(GYRO_PERIOD); - blue_led = LED_OFF; + printf("Starting Gyro\r\n"); initializeController(); break; case ANG_REF: - red_led = LED_ON; - green_led = LED_ON; - - reference = rcv.get_ang_ref();// - processMagAngle(); + set_leds_color(GREEN,red_led,green_led,blue_led); + reference = rcv.get_ang_ref() - processMagAngle(); printf("New reference: %f \n\r",reference*180/PI); if(reference > PI) reference = reference - 2*PI; - if(reference < -PI) + else if(reference < -PI) reference = reference + 2*PI; - red_led = LED_OFF; - green_led = LED_OFF; + break; + case CAM_ANG_REF: + set_leds_color(RED,red_led,green_led,blue_led); + reference = rcv.get_cam_ang_ref(); + printf("New reference: %f \n\r",reference*180/PI); break; case GND_VEL: - red_led = LED_ON; - blue_led = LED_ON; + set_leds_color(AQUA,red_led,green_led,blue_led); float vel = rcv.get_gnd_vel(); motor.setVelocity(vel); printf("GND_VEL = %f\r\n", vel); - - red_led = LED_OFF; - blue_led = LED_OFF; break; case JOG_VEL: - red_led = LED_ON; - blue_led = LED_ON; - + set_leds_color(WHITE,red_led,green_led,blue_led); float p, r; rcv.get_jog_vel(&p,&r); + printf("Joggin with period %f and duty cycle %f\r\n",p,r); if(p == 0 || r == 0) motor.stopJogging(); else motor.startJogging(r,p); - red_led = LED_OFF; - blue_led = LED_OFF; break; case PID_PARAMS: - blue_led = LED_ON; - green_led = LED_ON; - + set_leds_color(RED,red_led,green_led,blue_led); float ar[4]; rcv.get_pid_params(ar); P = ar[0]; @@ -218,26 +207,20 @@ ar[0], ar[1], ar[2], ar[3]); wait(1); - blue_led = LED_OFF; - green_led = LED_OFF; break; case MAG_CALIB: + set_leds_color(BLUE,red_led,green_led,blue_led); + printf("MAG_CALIB\r\n"); float mag[4]; rcv.get_mag_calib(mag); max_x=mag[1]; max_y=mag[3]; min_x=mag[0]; min_y=mag[2]; + printf(" max_x = %f, max_y= %f, min_x= %f, min_y= %f\r\n",mag[1],mag[3],mag[0],mag[2]); break; default: - blue_led = LED_ON; - green_led = LED_ON; - red_led = LED_ON; printf("nothing understood\r\n"); - wait(1); - blue_led = LED_OFF; - green_led = LED_OFF; - red_led = LED_OFF; //ser.printf("unknown command!\r\n"); } } @@ -312,8 +295,23 @@ /* Function to transform the magnetometer reading in angle(rad/s).*/ float processMagAngle(){ - mag.getAxis(mag_data); - x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1; - y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1; - return -atan2(y,x); + printf("starting ProcessMagAngle()\n\r"); + float mag_lpf = 0; + Timer t1; + for(int i = 0; i<20; i++){ + //printf("\r\n"); + //wait(0.1); + t1.start(); + __disable_irq(); + mag.getAxis(mag_data); + __enable_irq(); + t1.stop(); + x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1; + y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1; + mag_lpf = mag_lpf + (-atan2(y,x)); + wait(0.015); + } + // wait(20*0.01); + printf("Finished ProcessMagAngle() %d \n\r",t1.read_us()); + return mag_lpf/20; } \ No newline at end of file