Cooper Liu / Mbed 2 deprecated Eurobot2013_Co-Processor

working version with calibration done

Fork of Eurobot2013 by Oskar Weigl

main.cpp@10:2bd9f4e02b74, 2013-04-07 (annotated)

Committer:
xiaxia686
Date:
Sun Apr 07 16:30:49 2013 +0000
Revision:
10:2bd9f4e02b74
Parent:
9:08552997b544
Child:
11:5ba926692210
Working version with no serial communication to the main board. The IR sensors uses mode for identification;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 0:92019d8564a7 1 #include "mbed.h"
xiaxia686 10:2bd9f4e02b74 2 #include "rtos.h"
xiaxia686 10:2bd9f4e02b74 3 #include "math.h"
madcowswe 6:5a52c046d8f7 4 #include "globals.h"
xiaxia686 10:2bd9f4e02b74 5 #include "RFSRF05.h"
xiaxia686 10:2bd9f4e02b74 6 #include "PwmIn.h"
xiaxia686 10:2bd9f4e02b74 7 #include "system.h"
xiaxia686 10:2bd9f4e02b74 8 #include "geometryfuncs.h"
xiaxia686 10:2bd9f4e02b74 9
xiaxia686 10:2bd9f4e02b74 10
xiaxia686 10:2bd9f4e02b74 11 // Stepper motor pwm out
xiaxia686 10:2bd9f4e02b74 12 PwmOut stepper(STEPPER_PIN);
xiaxia686 10:2bd9f4e02b74 13 //InterruptIn stepper_irq(STEPPER_IRQ_PIN);
xiaxia686 10:2bd9f4e02b74 14
xiaxia686 10:2bd9f4e02b74 15 PwmIn ir_sensor(IR_SENSOR_PIN);
xiaxia686 10:2bd9f4e02b74 16
xiaxia686 10:2bd9f4e02b74 17 Serial pc(USBTX, USBRX); // tx, rx
xiaxia686 10:2bd9f4e02b74 18
xiaxia686 10:2bd9f4e02b74 19 RFSRF05 sonar(p16,p10,p11,p12,p13,p14,p15,p5,p6,p7,p8,p9);
xiaxia686 10:2bd9f4e02b74 20
xiaxia686 10:2bd9f4e02b74 21
xiaxia686 10:2bd9f4e02b74 22
xiaxia686 10:2bd9f4e02b74 23 // RTOS stuff
xiaxia686 10:2bd9f4e02b74 24 Semaphore serial_sema(1);
xiaxia686 10:2bd9f4e02b74 25 Semaphore IR_timeout_sema(1);
xiaxia686 10:2bd9f4e02b74 26
xiaxia686 10:2bd9f4e02b74 27 // IR timeout timer
xiaxia686 10:2bd9f4e02b74 28 Timeout IR_Timeout;
xiaxia686 10:2bd9f4e02b74 29
xiaxia686 10:2bd9f4e02b74 30 // some bad globals
xiaxia686 10:2bd9f4e02b74 31 float my_dutycycle;
xiaxia686 10:2bd9f4e02b74 32 float sonar_dist[3];
xiaxia686 10:2bd9f4e02b74 33 int IR_counter[3] = {0,0,0};
xiaxia686 10:2bd9f4e02b74 34 int IR_step_counter[3]= {0,0,0};
xiaxia686 10:2bd9f4e02b74 35 int sample_count = 0;
xiaxia686 10:2bd9f4e02b74 36 int step_counter;
xiaxia686 10:2bd9f4e02b74 37 float IR_angles[3];
xiaxia686 10:2bd9f4e02b74 38 int step_count_old = 0;
xiaxia686 10:2bd9f4e02b74 39
xiaxia686 10:2bd9f4e02b74 40 void IR_Timeout_isr() {
xiaxia686 10:2bd9f4e02b74 41 OLED4 = !OLED4;
xiaxia686 10:2bd9f4e02b74 42 IR_timeout_sema.wait();
xiaxia686 10:2bd9f4e02b74 43 if ((IR_counter[0] > IR_counter[1]) && (IR_counter[0] > IR_counter[2])) {
xiaxia686 10:2bd9f4e02b74 44 IR_angles[0] = rectifyAng(((float)IR_step_counter[0]/IR_counter[0])*STEP_ANGLE);
xiaxia686 10:2bd9f4e02b74 45 } else if ((IR_counter[1] > IR_counter[0]) && (IR_counter[1] > IR_counter[2])) {
xiaxia686 10:2bd9f4e02b74 46 IR_angles[1] = rectifyAng(((float)IR_step_counter[1]/IR_counter[1])*STEP_ANGLE);
xiaxia686 10:2bd9f4e02b74 47 } else if ((IR_counter[2] > IR_counter[0]) && (IR_counter[2] > IR_counter[1])) {
xiaxia686 10:2bd9f4e02b74 48 IR_angles[2] = rectifyAng(((float)IR_step_counter[2]/IR_counter[2])*STEP_ANGLE);
xiaxia686 10:2bd9f4e02b74 49 }
xiaxia686 10:2bd9f4e02b74 50 IR_counter[0] = 0;
xiaxia686 10:2bd9f4e02b74 51 IR_step_counter[0] = 0;
xiaxia686 10:2bd9f4e02b74 52 IR_counter[1] = 0;
xiaxia686 10:2bd9f4e02b74 53 IR_step_counter[1] = 0;
xiaxia686 10:2bd9f4e02b74 54 IR_counter[2] = 0;
xiaxia686 10:2bd9f4e02b74 55 IR_step_counter[2] = 0;
xiaxia686 10:2bd9f4e02b74 56 step_count_old = 0;
xiaxia686 10:2bd9f4e02b74 57
xiaxia686 10:2bd9f4e02b74 58 IR_timeout_sema.release();
xiaxia686 10:2bd9f4e02b74 59 serial_sema.release();
xiaxia686 10:2bd9f4e02b74 60 }
xiaxia686 10:2bd9f4e02b74 61
xiaxia686 10:2bd9f4e02b74 62
xiaxia686 10:2bd9f4e02b74 63
xiaxia686 10:2bd9f4e02b74 64 void IR_Callback(float pulsewidth)
xiaxia686 10:2bd9f4e02b74 65 {
xiaxia686 10:2bd9f4e02b74 66 static int step_count_local;
madcowswe 0:92019d8564a7 67
xiaxia686 10:2bd9f4e02b74 68 step_count_local = LPC_TIM2->TC; // save current counter value to a local
xiaxia686 10:2bd9f4e02b74 69 // resets timeout if pulse is captured
xiaxia686 10:2bd9f4e02b74 70 IR_Timeout.detach ();
xiaxia686 10:2bd9f4e02b74 71
xiaxia686 10:2bd9f4e02b74 72
xiaxia686 10:2bd9f4e02b74 73 if ( step_count_local < step_count_old ){
xiaxia686 10:2bd9f4e02b74 74 step_count_local += STEPPER_DIV;
xiaxia686 10:2bd9f4e02b74 75 }
xiaxia686 10:2bd9f4e02b74 76 OLED1 = !OLED1;
xiaxia686 10:2bd9f4e02b74 77 IR_timeout_sema.wait();
xiaxia686 10:2bd9f4e02b74 78 step_count_old = step_count_local;
xiaxia686 10:2bd9f4e02b74 79 if ((pulsewidth <= IR0_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth > IR0_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
xiaxia686 10:2bd9f4e02b74 80 IR_counter[0]++;
xiaxia686 10:2bd9f4e02b74 81 IR_step_counter[0] += step_count_local;
xiaxia686 10:2bd9f4e02b74 82 } else if ((pulsewidth <= IR1_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth > IR1_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
xiaxia686 10:2bd9f4e02b74 83 IR_counter[1]++;
xiaxia686 10:2bd9f4e02b74 84 IR_step_counter[1] += step_count_local;
xiaxia686 10:2bd9f4e02b74 85 } else if ((pulsewidth <= IR2_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth > IR2_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
xiaxia686 10:2bd9f4e02b74 86 IR_counter[2]++;
xiaxia686 10:2bd9f4e02b74 87 IR_step_counter[2] += step_count_local;
xiaxia686 10:2bd9f4e02b74 88 }
xiaxia686 10:2bd9f4e02b74 89 IR_timeout_sema.release();
xiaxia686 10:2bd9f4e02b74 90
xiaxia686 10:2bd9f4e02b74 91 IR_Timeout.attach(&IR_Timeout_isr, IR_TIMEOUT);
xiaxia686 10:2bd9f4e02b74 92 }
xiaxia686 10:2bd9f4e02b74 93
xiaxia686 10:2bd9f4e02b74 94
xiaxia686 10:2bd9f4e02b74 95 void Sonar_Callback(int num, float dist)
xiaxia686 10:2bd9f4e02b74 96 {
xiaxia686 10:2bd9f4e02b74 97 //Here is where you deal with your brand new reading ;D
xiaxia686 10:2bd9f4e02b74 98 OLED2 = !OLED2;
xiaxia686 10:2bd9f4e02b74 99 sonar_dist[num] = dist;
xiaxia686 10:2bd9f4e02b74 100 //serial_sema.release();
xiaxia686 10:2bd9f4e02b74 101
xiaxia686 10:2bd9f4e02b74 102 }
xiaxia686 10:2bd9f4e02b74 103
xiaxia686 10:2bd9f4e02b74 104
xiaxia686 10:2bd9f4e02b74 105
madcowswe 0:92019d8564a7 106
xiaxia686 10:2bd9f4e02b74 107 void serial_thread(void const *argument)
xiaxia686 10:2bd9f4e02b74 108 {
xiaxia686 10:2bd9f4e02b74 109 while (true) {
xiaxia686 10:2bd9f4e02b74 110 serial_sema.wait();
xiaxia686 10:2bd9f4e02b74 111 //printf("dutycycle: %0.4f, Sonar: %0.4f, %0.4f,%0.4f \n\r", my_dutycycle, sonar_dist[0],sonar_dist[1],sonar_dist[2]);
xiaxia686 10:2bd9f4e02b74 112 printf("IR0: %f, IR1: %f, IR2: %f \n\r",IR_angles[0],IR_angles[1],IR_angles[2]);
xiaxia686 10:2bd9f4e02b74 113 //printf("IR0: %d, IR1: %d, IR2: %d \n\r",IR_counter[0],IR_counter[1],IR_counter[2]);
xiaxia686 10:2bd9f4e02b74 114 }
madcowswe 0:92019d8564a7 115 }
xiaxia686 10:2bd9f4e02b74 116
xiaxia686 10:2bd9f4e02b74 117 int main()
xiaxia686 10:2bd9f4e02b74 118 {
xiaxia686 10:2bd9f4e02b74 119 pc.baud(19200);
xiaxia686 10:2bd9f4e02b74 120 pc.printf("Hello from mbed\n");
xiaxia686 10:2bd9f4e02b74 121
xiaxia686 10:2bd9f4e02b74 122 IR_timeout_sema.release();
xiaxia686 10:2bd9f4e02b74 123
xiaxia686 10:2bd9f4e02b74 124 sonar.callbackfunc = Sonar_Callback;
xiaxia686 10:2bd9f4e02b74 125 ir_sensor.callbackfunc = IR_Callback;
xiaxia686 10:2bd9f4e02b74 126
xiaxia686 10:2bd9f4e02b74 127 Thread thread_serial(serial_thread);
xiaxia686 10:2bd9f4e02b74 128
xiaxia686 10:2bd9f4e02b74 129 stepper.period(STEPPER_PERIOD);
xiaxia686 10:2bd9f4e02b74 130 stepper.pulsewidth(STEPPER_PERIOD/2.0f); // servo position determined by a pulsewidth between 1-2ms
xiaxia686 10:2bd9f4e02b74 131
xiaxia686 10:2bd9f4e02b74 132
xiaxia686 10:2bd9f4e02b74 133 //some timer counters
xiaxia686 10:2bd9f4e02b74 134 //enable PCTIM2
xiaxia686 10:2bd9f4e02b74 135 LPC_SC->PCONP|=(1<<22);
xiaxia686 10:2bd9f4e02b74 136
xiaxia686 10:2bd9f4e02b74 137 //SET P30
xiaxia686 10:2bd9f4e02b74 138 LPC_PINCON->PINSEL0|=((1<<8)|(1<<9));
xiaxia686 10:2bd9f4e02b74 139
xiaxia686 10:2bd9f4e02b74 140 //configure counter
xiaxia686 10:2bd9f4e02b74 141 LPC_TIM2->TCR =0x2;//counter disable
xiaxia686 10:2bd9f4e02b74 142 LPC_TIM2->CTCR =0x1;//counter mode,increments on rising edges
xiaxia686 10:2bd9f4e02b74 143 LPC_TIM2->PR =0x0;//set prescaler
xiaxia686 10:2bd9f4e02b74 144 LPC_TIM2->MR0 = STEPPER_DIV; // Match count for 3200 counts
xiaxia686 10:2bd9f4e02b74 145 LPC_TIM2->MCR = 2; // Reset on Match
xiaxia686 10:2bd9f4e02b74 146 LPC_TIM2->TCR =0x1; // counter enable
xiaxia686 10:2bd9f4e02b74 147
xiaxia686 10:2bd9f4e02b74 148 while (true) {
xiaxia686 10:2bd9f4e02b74 149 Thread::wait(osWaitForever);
xiaxia686 10:2bd9f4e02b74 150 }
xiaxia686 10:2bd9f4e02b74 151 }
xiaxia686 10:2bd9f4e02b74 152