ryo seki / Mbed 2 deprecated ver1_2_0

robocup

Dependencies:   HMC6352 PID mbed

main.cpp@0:13ab960fc61f, 2013-03-08 (annotated)

Committer:
akudohune
Date:
Fri Mar 08 07:13:29 2013 +0000
Revision:
0:13ab960fc61f
ver1_2_0;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akudohune 0:13ab960fc61f 1 #include <math.h>
akudohune 0:13ab960fc61f 2 #include <sstream>
akudohune 0:13ab960fc61f 3 #include "mbed.h"
akudohune 0:13ab960fc61f 4 #include "HMC6352.h"
akudohune 0:13ab960fc61f 5 #include "PID.h"
akudohune 0:13ab960fc61f 6 #include "main.h"
akudohune 0:13ab960fc61f 7
akudohune 0:13ab960fc61f 8
akudohune 0:13ab960fc61f 9 /*********** Serial interrupt ***********/
akudohune 0:13ab960fc61f 10
akudohune 0:13ab960fc61f 11 void Tx_interrupt()
akudohune 0:13ab960fc61f 12 {
akudohune 0:13ab960fc61f 13 array(speed[0],speed[1],speed[2],speed[3]);
akudohune 0:13ab960fc61f 14 driver.printf("%s",StringFIN.c_str());
akudohune 0:13ab960fc61f 15 //pc.printf("%s",StringFIN.c_str());
akudohune 0:13ab960fc61f 16 //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
akudohune 0:13ab960fc61f 17 }
akudohune 0:13ab960fc61f 18 /*
akudohune 0:13ab960fc61f 19 void Rx_interrupt()
akudohune 0:13ab960fc61f 20 {
akudohune 0:13ab960fc61f 21 if(driver.readable()){
akudohune 0:13ab960fc61f 22 //pc.printf("%d\n",driver.getc());
akudohune 0:13ab960fc61f 23 }
akudohune 0:13ab960fc61f 24 }*/
akudohune 0:13ab960fc61f 25
akudohune 0:13ab960fc61f 26
akudohune 0:13ab960fc61f 27 /*********** Serial interrupt end **********/
akudohune 0:13ab960fc61f 28
akudohune 0:13ab960fc61f 29 void PidUpdata()
akudohune 0:13ab960fc61f 30 {
akudohune 0:13ab960fc61f 31 float deviation = 0;
akudohune 0:13ab960fc61f 32
akudohune 0:13ab960fc61f 33
akudohune 0:13ab960fc61f 34 if(standard < 180.0){
akudohune 0:13ab960fc61f 35 if((compass.sample() / 10.0) < standard){
akudohune 0:13ab960fc61f 36 inputPID = 180.0 -(standard - (compass.sample() / 10.0));
akudohune 0:13ab960fc61f 37 }else if((compass.sample() / 10.0) < 180.0 + standard){
akudohune 0:13ab960fc61f 38 inputPID = 180.0 +((compass.sample() / 10.0) - standard);
akudohune 0:13ab960fc61f 39 }else{
akudohune 0:13ab960fc61f 40 inputPID = 180.0 - ((360.0 - (compass.sample() / 10.0)) + standard);
akudohune 0:13ab960fc61f 41 }
akudohune 0:13ab960fc61f 42 }else if(standard > 180.0){
akudohune 0:13ab960fc61f 43 if((compass.sample() / 10.0) > standard){
akudohune 0:13ab960fc61f 44 inputPID = 180.0 +((compass.sample() / 10.0) - standard);
akudohune 0:13ab960fc61f 45 }else if((compass.sample() / 10.0) > standard - 180.0){
akudohune 0:13ab960fc61f 46 inputPID = 180.0 -(standard - (compass.sample() / 10.0));
akudohune 0:13ab960fc61f 47 }else{
akudohune 0:13ab960fc61f 48 inputPID = 180.0 + ((compass.sample() / 10.0) + (360.0 - standard));
akudohune 0:13ab960fc61f 49 }
akudohune 0:13ab960fc61f 50 }else{
akudohune 0:13ab960fc61f 51 inputPID = compass.sample() / 10.0;
akudohune 0:13ab960fc61f 52 }
akudohune 0:13ab960fc61f 53
akudohune 0:13ab960fc61f 54 if(inputPID < 0){
akudohune 0:13ab960fc61f 55 inputPID *= -1; //
akudohune 0:13ab960fc61f 56 }
akudohune 0:13ab960fc61f 57
akudohune 0:13ab960fc61f 58
akudohune 0:13ab960fc61f 59 //pid.setProcessValue(inputPID);
akudohune 0:13ab960fc61f 60
akudohune 0:13ab960fc61f 61 deviation = (inputPID - 180.0);
akudohune 0:13ab960fc61f 62
akudohune 0:13ab960fc61f 63 compassPID = ((deviation / 180.0) * 20.0 + ((deviation - lastData) / 2.0));
akudohune 0:13ab960fc61f 64
akudohune 0:13ab960fc61f 65 if(compassPID > 100)compassPID = 100;
akudohune 0:13ab960fc61f 66 else if(compassPID < -100)compassPID = -100;
akudohune 0:13ab960fc61f 67
akudohune 0:13ab960fc61f 68 //pc.printf("%f\n",compassPID);
akudohune 0:13ab960fc61f 69
akudohune 0:13ab960fc61f 70 lastData = deviation;
akudohune 0:13ab960fc61f 71 }
akudohune 0:13ab960fc61f 72
akudohune 0:13ab960fc61f 73
akudohune 0:13ab960fc61f 74
akudohune 0:13ab960fc61f 75 void move(int vx, int vy, int vs)
akudohune 0:13ab960fc61f 76 {
akudohune 0:13ab960fc61f 77 double motVal[MOT_NUM] = {0};
akudohune 0:13ab960fc61f 78
akudohune 0:13ab960fc61f 79 motVal[0] = (double)((0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs));
akudohune 0:13ab960fc61f 80 motVal[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs));
akudohune 0:13ab960fc61f 81 motVal[2] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs));
akudohune 0:13ab960fc61f 82 motVal[3] = (double)((0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs));
akudohune 0:13ab960fc61f 83
akudohune 0:13ab960fc61f 84 for(uint8_t i = 0;i < MOT_NUM;i++){
akudohune 0:13ab960fc61f 85 if(motVal[i] > 100)motVal[i] = 100;
akudohune 0:13ab960fc61f 86 else if(motVal[i] < -100)motVal[i] = -100;
akudohune 0:13ab960fc61f 87 speed[i] = motVal[i];
akudohune 0:13ab960fc61f 88 }
akudohune 0:13ab960fc61f 89 /*
akudohune 0:13ab960fc61f 90 pc.printf("speed1 = %d\n",speed[0]);
akudohune 0:13ab960fc61f 91 pc.printf("speed2 = %d\n",speed[1]);
akudohune 0:13ab960fc61f 92 pc.printf("speed3 = %d\n",speed[2]);
akudohune 0:13ab960fc61f 93 pc.printf("speed4 = %d\n\n",speed[3]);
akudohune 0:13ab960fc61f 94 */
akudohune 0:13ab960fc61f 95 ////pc.printf("%s",StringFIN.c_str());
akudohune 0:13ab960fc61f 96 }
akudohune 0:13ab960fc61f 97
akudohune 0:13ab960fc61f 98 void init()
akudohune 0:13ab960fc61f 99 {
akudohune 0:13ab960fc61f 100
akudohune 0:13ab960fc61f 101 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
akudohune 0:13ab960fc61f 102 StartButton.mode(PullUp);
akudohune 0:13ab960fc61f 103 CalibEnterButton.mode(PullUp);
akudohune 0:13ab960fc61f 104 CalibExitButton.mode(PullUp);
akudohune 0:13ab960fc61f 105 driver.baud(BAUD_RATE);
akudohune 0:13ab960fc61f 106 wait_ms(MOTDRIVER_WAIT);
akudohune 0:13ab960fc61f 107 driver.printf("1F0002F0003F0004F000\r\n");
akudohune 0:13ab960fc61f 108
akudohune 0:13ab960fc61f 109 led1 = ON;
akudohune 0:13ab960fc61f 110
akudohune 0:13ab960fc61f 111 while(StartButton){
akudohune 0:13ab960fc61f 112 if(!CalibEnterButton){
akudohune 0:13ab960fc61f 113 led1 = OFF;
akudohune 0:13ab960fc61f 114 led2 = ON;
akudohune 0:13ab960fc61f 115 compass.setCalibrationMode(ENTER);
akudohune 0:13ab960fc61f 116 while(CalibExitButton);
akudohune 0:13ab960fc61f 117 compass.setCalibrationMode(EXIT);
akudohune 0:13ab960fc61f 118 led2 = OFF;
akudohune 0:13ab960fc61f 119 led3 = ON;
akudohune 0:13ab960fc61f 120 }
akudohune 0:13ab960fc61f 121 }
akudohune 0:13ab960fc61f 122
akudohune 0:13ab960fc61f 123 standard = compass.sample() / 10.0;
akudohune 0:13ab960fc61f 124 led1 = OFF;
akudohune 0:13ab960fc61f 125 led3 = OFF;
akudohune 0:13ab960fc61f 126
akudohune 0:13ab960fc61f 127 pid.setInputLimits(0.0, 360.0);
akudohune 0:13ab960fc61f 128 pid.setOutputLimits(-50.0, 50.0);
akudohune 0:13ab960fc61f 129 pid.setBias(0.0);
akudohune 0:13ab960fc61f 130 pid.setMode(AUTO_MODE);
akudohune 0:13ab960fc61f 131 pid.setSetPoint(180.0);
akudohune 0:13ab960fc61f 132
akudohune 0:13ab960fc61f 133 pidUpdata.attach(&PidUpdata, 0.06);
akudohune 0:13ab960fc61f 134 //ultrasonic.attach(&Ultrasonic, 0.1);
akudohune 0:13ab960fc61f 135 IR.attach(&IR_Position,0.04);
akudohune 0:13ab960fc61f 136 driver.attach(&Tx_interrupt, Serial::TxIrq);
akudohune 0:13ab960fc61f 137 //driver.attach(&Rx_interrupt, Serial::RxIrq);
akudohune 0:13ab960fc61f 138
akudohune 0:13ab960fc61f 139 timer2.start();
akudohune 0:13ab960fc61f 140 }
akudohune 0:13ab960fc61f 141
akudohune 0:13ab960fc61f 142 int main()
akudohune 0:13ab960fc61f 143 {
akudohune 0:13ab960fc61f 144 int vx=0,vy=0,vs=0;
akudohune 0:13ab960fc61f 145
akudohune 0:13ab960fc61f 146 init();
akudohune 0:13ab960fc61f 147
akudohune 0:13ab960fc61f 148 while(1) {
akudohune 0:13ab960fc61f 149 vs = compassPID;
akudohune 0:13ab960fc61f 150
akudohune 0:13ab960fc61f 151 if(IR_found){
akudohune 0:13ab960fc61f 152 if((direction == 4) || (direction == 12)||(direction == 3) || (direction == 5) || (direction ==11) || (direction == 13)){
akudohune 0:13ab960fc61f 153 vx = (int)(20*ball_sankaku[direction][0]);
akudohune 0:13ab960fc61f 154 vy = (int)(20*ball_sankaku[direction][1]);
akudohune 0:13ab960fc61f 155 }else{
akudohune 0:13ab960fc61f 156 vx = (int)(10*ball_sankaku[direction][0]);
akudohune 0:13ab960fc61f 157 vy = (int)(10*ball_sankaku[direction][1]);
akudohune 0:13ab960fc61f 158 }
akudohune 0:13ab960fc61f 159
akudohune 0:13ab960fc61f 160 if(Distance <= 10){
akudohune 0:13ab960fc61f 161 vx *= -1;
akudohune 0:13ab960fc61f 162 vy *= -1;
akudohune 0:13ab960fc61f 163 }
akudohune 0:13ab960fc61f 164 }else{
akudohune 0:13ab960fc61f 165 vx = 0;
akudohune 0:13ab960fc61f 166 vy = 0;
akudohune 0:13ab960fc61f 167 }
akudohune 0:13ab960fc61f 168
akudohune 0:13ab960fc61f 169
akudohune 0:13ab960fc61f 170 move(vx,vy,vs);
akudohune 0:13ab960fc61f 171 }
akudohune 0:13ab960fc61f 172 }