epuckaseba.c
Go to the documentation of this file.
00001 /*
00002         Aseba - an event-based framework for distributed robot control
00003         Copyright (C) 2007--2010:
00004                 Stephane Magnenat <stephane at magnenat dot net>
00005                 (http://stephane.magnenat.net)
00006                 and other contributors, see authors.txt for details
00007         
00008         This program is free software: you can redistribute it and/or modify
00009         it under the terms of the GNU Lesser General Public License as published
00010         by the Free Software Foundation, version 3 of the License.
00011         
00012         This program is distributed in the hope that it will be useful,
00013         but WITHOUT ANY WARRANTY; without even the implied warranty of
00014         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015         GNU Lesser General Public License for more details.
00016         
00017         You should have received a copy of the GNU Lesser General Public License
00018         along with this program. If not, see <http://www.gnu.org/licenses/>.
00019 */
00020 
00021 /*
00022         WARNING: you have to change the size of the UART 1 e-puck reception buffer to hold
00023         at the biggest possible packet (probably bytecode + header). Otherwise if you set
00024         a new bytecode while busy (for instance while sending description), you might end up
00025         in a dead-lock.
00026 */
00027 
00028 #include <p30f6014a.h>
00029 
00030 #include "e-puck/library/motor_led/e_epuck_ports.h"
00031 #include "e-puck/library/motor_led/e_init_port.h"
00032 #include "e-puck/library/motor_led/e_led.h"
00033 #include "e-puck/library/motor_led/advance_one_timer/e_motors.h"
00034 #include "e-puck/library/motor_led/advance_one_timer/e_agenda.h"
00035 #include "e-puck/library/camera/fast_2_timer/e_poxxxx.h"
00036 #include "e-puck/library/uart/e_uart_char.h"
00037 #include "e-puck/library/a_d/advance_ad_scan/e_ad_conv.h"
00038 #include "e-puck/library/a_d/advance_ad_scan/e_prox.h"
00039 #include "e-puck/library/a_d/advance_ad_scan/e_acc.h"
00040 #include "e-puck/library/I2C/e_I2C_protocol.h"
00041 
00042 #include "libpic30.h"
00043 
00044 
00045 #include <string.h>
00046 
00047 #define ASEBA_ASSERT
00048 #include <vm/vm.h>
00049 #include <common/consts.h>
00050 #include <vm/natives.h>
00051 #include <transport/buffer/vm-buffer.h>
00052 
00053 #define LIS_GROUND_SENSORS
00054 
00055 #define vmVariablesSize (sizeof(struct EPuckVariables) / sizeof(sint16))
00056 #define vmStackSize 32
00057 #define argsSize 32
00058 
00059 
00060 unsigned int __attribute((far)) cam_data[60];
00061 
00062 
00063 // we receive the data as big endian and read them as little, so we have to acces the bit the right way
00064 #define CAM_RED(pixel) ( 3 * (((pixel) >> 3) & 0x1f))
00065 
00066 #define CAM_GREEN(pixel) ((3 * ((((pixel) & 0x7) << 3) | ((pixel) >> 13))) >> 1)
00067 
00068 #define CAM_BLUE(pixel) ( 3 * (((pixel) >> 8) & 0x1f))
00069 
00070 #define CLAMP(v, vmin, vmax) ((v) < (vmin) ? (vmin) : (v) > (vmax) ? (vmax) : (v))
00071 
00072 // data
00073 
00074 /*** In your code, put "SET_EVENT(EVENT_NUMBER)" when you want to trigger an 
00075          event. This macro is interrupt-safe, you can call it anywhere you want.
00076 ***/
00077 #define SET_EVENT(event) do {events_flags |= 1 << event;} while(0)
00078 #define CLEAR_EVENT(event) do {events_flags &= ~(1 << event);} while(0)
00079 #define IS_EVENT(event) (events_flags & (1 << event))
00080 
00081 /* VM */
00082 
00083 /* The number of opcode an aseba script can have */
00084 #define VM_BYTECODE_SIZE 1024
00085 #define VM_STACK_SIZE 32
00086 
00087 int _EEDATA(1) bytecode_version;
00088 unsigned char _EEDATA(1) eeprom_bytecode[VM_BYTECODE_SIZE*2];
00089 
00090 struct EPuckVariables
00091 {
00092         // NodeID
00093         sint16 id;
00094         // source
00095         sint16 source;
00096         // args
00097         sint16 args[argsSize];
00098         // motor
00099         sint16 leftSpeed;
00100         sint16 rightSpeed;
00101         // leds
00102         sint16 leds[8];
00103         // prox
00104         sint16 prox[8];
00105         sint16 ambiant[8];
00106         // acc
00107         sint16 acc[3];
00108         // camera
00109         sint16 camLine;
00110         sint16 camR[60];
00111         sint16 camG[60];
00112         sint16 camB[60];
00113         // free space
00114         sint16 freeSpace[128];
00115 } __attribute__ ((far)) ePuckVariables;
00116 
00117 char name[] = "e-puck0";
00118 
00119 AsebaVMDescription vmDescription = {
00120         name,   // Name of the microcontroller
00121         {
00122                 { 1, "id" },    // Do not touch it
00123                 { 1, "source" }, // nor this one
00124                 { argsSize, "args" },   // neither this one
00125                 {1, "leftSpeed"},
00126                 {1, "rightSpeed"},
00127         // leds
00128                 {8, "leds"},
00129         // prox
00130             {8, "prox"},
00131                 {8, "ambiant"},
00132         // acc
00133                 {3, "acc"},
00134         // camera
00135                 {1, "camLine"},
00136                 {60, "camR"},
00137                 {60, "camG"},
00138                 {60, "camB"},
00139                 
00140                 { 0, NULL }     // null terminated
00141         }
00142 };
00143 
00144 static uint16 vmBytecode[VM_BYTECODE_SIZE];
00145 
00146 static sint16 vmStack[VM_STACK_SIZE];
00147 
00148 static AsebaVMState vmState = {
00149         0x1,
00150         
00151         VM_BYTECODE_SIZE,
00152         vmBytecode,
00153         
00154         sizeof(ePuckVariables) / sizeof(sint16),
00155         (sint16*)&ePuckVariables,
00156 
00157         VM_STACK_SIZE,
00158         vmStack
00159 };
00160 
00161 const AsebaVMDescription* AsebaGetVMDescription(AsebaVMState *vm)
00162 {
00163         return &vmDescription;
00164 }       
00165 
00166 
00167 
00168 
00169 static unsigned int events_flags = 0;
00170 enum Events
00171 {
00172         EVENT_IR_SENSORS = 0,
00173         EVENT_CAMERA,
00174         EVENTS_COUNT
00175 };
00176 
00177 static const AsebaLocalEventDescription localEvents[] = { 
00178         {"ir_sensors", "New IR sensors values available"},
00179         {"camera", "New camera picture available"},
00180         { NULL, NULL }
00181 };
00182 
00183 const AsebaLocalEventDescription * AsebaGetLocalEventsDescriptions(AsebaVMState *vm)
00184 {
00185         return localEvents;
00186 }
00187 
00188 #ifdef LIS_GROUND_SENSORS 
00189 void EpuckNative_get_ground_values(AsebaVMState *vm)
00190 {
00191         // where to 
00192         uint16 dest = AsebaNativePopArg(vm);
00193         uint16 i;
00194         e_i2cp_enable();
00195 
00196         for (i = 0; i < 3; i++)
00197         {       
00198                 unsigned int temp;
00199                 temp = e_i2cp_read(0xC0,i*2) << 8;
00200                 temp |= e_i2cp_read(0xC0,i*2+1);
00201                 vm->variables[dest++] = temp;
00202         }
00203         e_i2cp_disable();
00204 }
00205 
00206 AsebaNativeFunctionDescription EpuckNativeDescription_get_ground_values =
00207 {
00208         "get_ground_values",
00209         "read the values of the ground sensors",
00210         {
00211                 { 3, "dest" },
00212                 { 0, 0 }
00213         }
00214 };
00215 #endif
00216 
00217 static const AsebaNativeFunctionDescription* nativeFunctionsDescription[] = {
00218         ASEBA_NATIVES_STD_DESCRIPTIONS,
00219 #ifdef LIS_GROUND_SENSORS
00220         &EpuckNativeDescription_get_ground_values,
00221 #endif
00222         0       // null terminated
00223 };
00224 
00225 static AsebaNativeFunctionPointer nativeFunctions[] = {
00226         ASEBA_NATIVES_STD_FUNCTIONS,
00227 #ifdef LIS_GROUND_SENSORS
00228         EpuckNative_get_ground_values,
00229 #endif
00230 };
00231 
00232 void AsebaPutVmToSleep(AsebaVMState *vm)
00233 {
00234 }
00235 
00236 void AsebaNativeFunction(AsebaVMState *vm, uint16 id)
00237 {
00238         nativeFunctions[id](vm);
00239 }
00240 
00241 const AsebaNativeFunctionDescription * const * AsebaGetNativeFunctionsDescriptions(AsebaVMState *vm)
00242 {
00243         return nativeFunctionsDescription;
00244 }       
00245 
00246 void uartSendUInt8(uint8 value)
00247 {
00248         e_send_uart1_char((char *)&value, 1);
00249         while (e_uart1_sending());
00250 }
00251 
00252 void uartSendUInt16(uint16 value)
00253 {
00254         e_send_uart1_char((char *)&value, 2);
00255         while (e_uart1_sending());
00256 }
00257 
00258 void AsebaSendBuffer(AsebaVMState *vm, const uint8* data, uint16 length)
00259 {
00260         uartSendUInt16(length - 2);
00261         uartSendUInt16(vmState.nodeId);
00262         uint16 i;
00263         for (i = 0; i < length; i++)
00264                 uartSendUInt8(*data++);
00265 }       
00266 
00267 uint8 uartGetUInt8()
00268 {
00269         char c;
00270         while (!e_ischar_uart1());
00271         e_getchar_uart1(&c);
00272         return c;
00273 }
00274 
00275 uint16 uartGetUInt16()
00276 {
00277         uint16 value;
00278         // little endian
00279         value = uartGetUInt8();
00280         value |= (uartGetUInt8() << 8);
00281         return value;
00282 }
00283 
00284 uint16 AsebaGetBuffer(AsebaVMState *vm, uint8* data, uint16 maxLength, uint16* source)
00285 {
00286         BODY_LED = 1;
00287         uint16 ret = 0;
00288         if (e_ischar_uart1())
00289         {
00290                 uint16 len = uartGetUInt16() + 2;
00291                 *source = uartGetUInt16();
00292         
00293                 uint16 i;
00294                 for (i = 0; i < len; i++)
00295                         *data++ = uartGetUInt8();
00296                 ret = len;
00297         }
00298         BODY_LED = 0;
00299         return ret;
00300 }       
00301 
00302 
00303 extern int e_ambient_and_reflected_ir[8];
00304 void updateRobotVariables()
00305 {
00306         unsigned i;
00307         static int camline = 640/2-4;
00308         // motor
00309         static int leftSpeed = 0, rightSpeed = 0;
00310 
00311         if (ePuckVariables.leftSpeed != leftSpeed)
00312         {
00313                 leftSpeed = CLAMP(ePuckVariables.leftSpeed, -1000, 1000);
00314                 e_set_speed_left(leftSpeed);
00315         }
00316         if (ePuckVariables.rightSpeed != rightSpeed)
00317         {
00318                 rightSpeed = CLAMP(ePuckVariables.rightSpeed, -1000, 1000);
00319                 e_set_speed_right(rightSpeed);
00320         }
00321         
00322         
00323         if(e_ambient_and_reflected_ir[7] != 0xFFFF) {
00324                 SET_EVENT(EVENT_IR_SENSORS); 
00325                 // leds and prox
00326                 for (i = 0; i < 8; i++)
00327                 {
00328                         e_set_led(i, ePuckVariables.leds[i] ? 1 : 0);
00329                         ePuckVariables.ambiant[i] = e_get_ambient_light(i);
00330                         ePuckVariables.prox[i] =  e_get_calibrated_prox(i);
00331                 }
00332                 e_ambient_and_reflected_ir[7] = 0xFFFF;
00333         }
00334         
00335         for(i = 0; i < 3; i++)
00336                 ePuckVariables.acc[i] = e_get_acc(i);
00337 
00338         // camera
00339         if(e_poxxxx_is_img_ready())     
00340         {
00341                 for(i = 0; i < 60; i++)
00342                 {
00343                         ePuckVariables.camR[i] = CAM_RED(cam_data[i]);
00344                         ePuckVariables.camG[i] = CAM_GREEN(cam_data[i]);
00345                         ePuckVariables.camB[i] = CAM_BLUE(cam_data[i]);
00346                 }
00347                 if(camline != ePuckVariables.camLine)
00348                 {
00349                         camline = ePuckVariables.camLine;
00350                         e_poxxxx_config_cam(camline,0,4,480,4,8,RGB_565_MODE);
00351                 //      e_po3030k_set_ref_exposure(160);
00352                 //      e_po3030k_set_ww(0);
00353                         e_poxxxx_set_mirror(1,1);
00354                         e_poxxxx_write_cam_registers();
00355                 }
00356                 e_poxxxx_launch_capture((char *)cam_data);
00357                 SET_EVENT(EVENT_CAMERA);
00358         }
00359 }
00360 
00361 
00362 
00363 void AsebaAssert(AsebaVMState *vm, AsebaAssertReason reason)
00364 {
00365         e_set_body_led(1);
00366         while (1);
00367 }
00368 
00369 void initRobot()
00370 {
00371         // init stuff
00372         e_init_port();
00373         e_start_agendas_processing();
00374         e_init_motors();
00375         e_init_uart1();
00376         e_init_ad_scan(0);
00377         e_poxxxx_init_cam();
00378         e_poxxxx_config_cam(640/2-4,0,4,480,4,8,RGB_565_MODE);
00379 //      e_po3030k_set_ref_exposure(160);
00380 //      e_po3030k_set_ww(0);
00381         e_poxxxx_set_mirror(1,1);
00382         e_poxxxx_write_cam_registers();
00383         e_poxxxx_launch_capture((char *)cam_data);
00384 
00385         // reset if power on (some problem for few robots)
00386         if (RCONbits.POR)
00387         {
00388                 RCONbits.POR = 0;
00389                 RESET();
00390         }
00391 }
00392 
00393 
00394 void AsebaWriteBytecode(AsebaVMState *vm) {
00395         int i = 0;
00396         _prog_addressT EE_addr;
00397                 
00398         _init_prog_address(EE_addr, bytecode_version);
00399         _erase_eedata(EE_addr, 2);
00400         _wait_eedata();
00401         
00402         i = ASEBA_PROTOCOL_VERSION;
00403         _write_eedata_word(EE_addr, i);
00404                 
00405         _wait_eedata();
00406         
00407         _init_prog_address(EE_addr, eeprom_bytecode); 
00408         
00409         for(i = 0; i < 2048; i+=2) {
00410                 
00411                 _erase_eedata(EE_addr, 2);
00412                 _wait_eedata();
00413                 
00414                 _write_eedata_word(EE_addr, vm->bytecode[i/2]);
00415                 _wait_eedata();
00416                 
00417                 EE_addr += 2;
00418         }
00419 }
00420 void AsebaResetIntoBootloader(AsebaVMState *vm) {
00421         asm __volatile__ ("reset");
00422 }
00423 
00424 void initAseba()
00425 {
00426         // VM
00427         int selector = SELECTOR0 | (SELECTOR1 << 1) | (SELECTOR2 << 2);
00428         vmState.nodeId = selector + 1;
00429         AsebaVMInit(&vmState);
00430         ePuckVariables.id = selector + 1;
00431         ePuckVariables.camLine = 640/2-4;
00432         name[6] = '0' + selector;
00433         e_led_clear();
00434         e_set_led(selector,1);
00435 }
00436 
00437 int main()
00438 {       
00439         int i;
00440         _prog_addressT EE_addr;
00441         
00442         initRobot();
00443 
00444 //      do {
00445                 initAseba();
00446 //      } while(!e_ischar_uart1());
00447 /*
00448         for (i = 0; i < 14; i++)
00449         {
00450                 uartGetUInt8();
00451         }
00452 */
00453         e_calibrate_ir();
00454         e_acc_calibr();
00455 
00456         // Load the bytecode...
00457         _init_prog_address(EE_addr, bytecode_version);
00458         _memcpy_p2d16(&i, EE_addr, _EE_WORD);
00459         
00460         // ...only load bytecode if version is the same as current one
00461         if(i == ASEBA_PROTOCOL_VERSION)
00462         {
00463                 _init_prog_address(EE_addr, eeprom_bytecode);
00464                 
00465                 for( i = 0; i< 2048; i += 2)
00466                 {
00467                         _memcpy_p2d16(vmState.bytecode + i/2 , EE_addr, 2);
00468                         EE_addr += 2;
00469                 }
00470                 
00471                 // Init the vm
00472                 AsebaVMSetupEvent(&vmState, ASEBA_EVENT_INIT);  
00473         }
00474         
00475         while (1)
00476         {
00477                 AsebaProcessIncomingEvents(&vmState);           
00478                 updateRobotVariables();
00479                 AsebaVMRun(&vmState, 1000);
00480                 
00481                 if (AsebaMaskIsClear(vmState.flags, ASEBA_VM_STEP_BY_STEP_MASK) || AsebaMaskIsClear(vmState.flags, ASEBA_VM_EVENT_ACTIVE_MASK))
00482                 {
00483                         unsigned i;
00484                         // Find first bit from right (LSB) 
00485                         asm ("ff1r %[word], %[b]" : [b] "=r" (i) : [word] "r" (events_flags) : "cc");
00486                         if(i)
00487                         {
00488                                 i--;
00489                                 CLEAR_EVENT(i);
00490                                 ePuckVariables.source = vmState.nodeId;
00491                                 AsebaVMSetupEvent(&vmState, ASEBA_EVENT_LOCAL_EVENTS_START - i);
00492                         }
00493                 }
00494         }
00495         
00496         return 0;
00497 }
00498 


aseba
Author(s): Stéphane Magnenat
autogenerated on Thu Jan 2 2014 11:17:16